root / trunk / code / projects / colonet / testing / robot_routine_reg_test / servo.c @ 13
History | View | Annotate | Download (3.02 KB)
1 | 13 | emarinel | /*
|
---|---|---|---|
2 | servo.c - Contains functions and interrupts necessary to activate servos
|
||
3 | author: Tom Lauwers
|
||
4 | |||
5 | 3/1/06 Iain
|
||
6 | Man, I don't know what a servo is.
|
||
7 | */
|
||
8 | #include <avr/io.h> |
||
9 | #include <avr/signal.h> |
||
10 | #include "servo.h" |
||
11 | #include "dio.h" |
||
12 | #include "lights.h" |
||
13 | #include "lcd.h" |
||
14 | //16000 c/ms
|
||
15 | //i dunno must be 2^k
|
||
16 | //9C40 ~ 20ms
|
||
17 | //#A000 ~ 20.48 but prolly wont work
|
||
18 | #define SERVO_PERIOD 0xA000 |
||
19 | //needs be 1ms
|
||
20 | //is 1.024ms
|
||
21 | #define SERVO_CONSTANT 0x800 |
||
22 | |||
23 | /* dirty dirty
|
||
24 | unsigned int servo_vals[4] = {0,0,0,0}; // Stores the set servo values
|
||
25 | int current_servo = 0; // Stores which servo is current in the interrupt routine
|
||
26 | */
|
||
27 | unsigned int servo_vals[8] = {0,0,0,0,0,0,0,0}; // Stores the set servo values |
||
28 | int phase_time=0; |
||
29 | int phase_base=0; |
||
30 | int servos_on = 0; // Stores which servos are enabled |
||
31 | int servos_enabled=0; //which are high |
||
32 | int servo_flag = 0; // Stores whether or not servos are enabled |
||
33 | |||
34 | /* Timer 1 output compare B interrupt. Does the following:
|
||
35 | Checks to see if the current servo is 4, in which case just need
|
||
36 | to set low servo 3's signal and return.
|
||
37 | Else, check to see if the current servo is enabled, and set it's signal high
|
||
38 | Then, set the previous servo's signal low
|
||
39 | Now, set the next output compare to occur for some time from now
|
||
40 | specified by the current servo's value
|
||
41 | */
|
||
42 | SIGNAL (SIG_OUTPUT_COMPARE3B) |
||
43 | { |
||
44 | lcd_putchar('i');
|
||
45 | if(servo_flag == 0) |
||
46 | return;
|
||
47 | |||
48 | int i;
|
||
49 | if(phase_time == 0){ |
||
50 | lcd_putchar('p');
|
||
51 | for(i=0;i<8;i++){ |
||
52 | if(servos_on & _BV(i))
|
||
53 | digital_output((_PORT_E << 3) +i,1); |
||
54 | } |
||
55 | servos_enabled = servos_on; |
||
56 | phase_base = OCR3B; |
||
57 | } |
||
58 | |||
59 | unsigned int min=~0; |
||
60 | for(i=0;i<8;i++){ |
||
61 | if(_BV(i) & servos_enabled){
|
||
62 | if(SERVO_CONSTANT*servo_vals[i] <= phase_time){
|
||
63 | digital_output((_PORT_E << 3) + i,0); |
||
64 | servos_enabled &= ~_BV(i); |
||
65 | } |
||
66 | else if(SERVO_CONSTANT*servo_vals[i] < min) { |
||
67 | min = servo_vals[i]; |
||
68 | } |
||
69 | } |
||
70 | } |
||
71 | if(!servos_enabled){
|
||
72 | OCR3B = phase_base + SERVO_PERIOD; |
||
73 | phase_time = 0;
|
||
74 | } |
||
75 | else {
|
||
76 | OCR3B = phase_base + min; |
||
77 | phase_time = min; |
||
78 | } |
||
79 | |||
80 | } |
||
81 | |||
82 | //currently the light must be initted b4
|
||
83 | void init_servo()
|
||
84 | { |
||
85 | //1010 1001
|
||
86 | TCCR3A = 0; // COM## set to noninvert, WGM 1:0 set to 1 |
||
87 | //0000 1100
|
||
88 | TCCR3B = 0x02; // High bits to 0, WGM 3:2 set to 1, prescaler to 8 |
||
89 | |||
90 | lcd_init(); |
||
91 | ETIMSK |= 0x08;
|
||
92 | servo_flag = 1;
|
||
93 | OCR3B = 0x80;
|
||
94 | lcd_putchar('i');
|
||
95 | } |
||
96 | |||
97 | // Enable a servo specified by config
|
||
98 | void enable_servo(int config) |
||
99 | { |
||
100 | servos_on |= _BV(config); |
||
101 | } |
||
102 | |||
103 | // Disable a servo specified by config
|
||
104 | void disable_servo(int config) |
||
105 | { |
||
106 | servos_on &= (0xFF-_BV(config));
|
||
107 | } |
||
108 | |||
109 | // Disables the timer1 interrupt, disabling the servos
|
||
110 | void disable_servos()
|
||
111 | { |
||
112 | // Disables interrupts
|
||
113 | ETIMSK &= ~0x08;
|
||
114 | servo_flag=0;
|
||
115 | |||
116 | } |
||
117 | |||
118 | // Enables the timer1 interrupt, enabling the servos
|
||
119 | void enable_servos()
|
||
120 | { |
||
121 | // Enable interrupts on output compare B
|
||
122 | ETIMSK |= 0x08;
|
||
123 | servo_flag=1;
|
||
124 | } |
||
125 | // Set a servo to a value. Automatically enables the servo.
|
||
126 | void set_servo(int servo, int value) |
||
127 | { |
||
128 | enable_servo(servo); |
||
129 | servo_vals[servo] = value; |
||
130 | } |