root / trunk / code / projects / colonet / utilities / dragonfly_wireless_relay / servo.c @ 396
History | View | Annotate | Download (5.44 KB)
1 | 13 | emarinel | #include "servo.h" |
---|---|---|---|
2 | |||
3 | |||
4 | #define SERVO_COUNT 16 //please dont change this, or bad things might happen |
||
5 | |||
6 | |||
7 | |||
8 | struct SERVO_NODE {
|
||
9 | uint8_t num; |
||
10 | uint16_t angle; |
||
11 | }; |
||
12 | |||
13 | struct SERVO_CHANGE {
|
||
14 | uint16_t port_val; |
||
15 | uint16_t split_time_period; |
||
16 | }; |
||
17 | |||
18 | |||
19 | struct SERVO_STATUS_STRUCT {
|
||
20 | struct SERVO_NODE servos[SERVO_COUNT];
|
||
21 | uint16_t servo_angles[SERVO_COUNT]; |
||
22 | struct SERVO_CHANGE changes[SERVO_COUNT+1]; |
||
23 | uint8_t change_count; |
||
24 | uint16_t ms; |
||
25 | uint8_t new_angles; |
||
26 | uint8_t in_pulse; |
||
27 | uint8_t current_servo; |
||
28 | |||
29 | } SERVO_STATUS; |
||
30 | |||
31 | |||
32 | void servo_sort(void); |
||
33 | void servo_setup_pulse(void); |
||
34 | void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn); |
||
35 | |||
36 | |||
37 | |||
38 | |||
39 | SIGNAL (SIG_OUTPUT_COMPARE3B){ |
||
40 | |||
41 | if (SERVO_STATUS.in_pulse) {
|
||
42 | |||
43 | //pull the correct ones down
|
||
44 | SERVOPORT1 &= (~SERVOMASK1)|SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val; |
||
45 | SERVOPORT2 &= (~SERVOMASK2)|(SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val >> 8);
|
||
46 | |||
47 | ++SERVO_STATUS.current_servo; //now look at next servo
|
||
48 | |||
49 | if (SERVO_STATUS.current_servo < SERVO_STATUS.change_count) { //if it isnt the end... |
||
50 | |||
51 | //setup timer for next pull down
|
||
52 | OCR3B = SERVO_STATUS.changes[SERVO_STATUS.current_servo].split_time_period; |
||
53 | TCNT3 = 0;
|
||
54 | } |
||
55 | else { //we are done with these pulses |
||
56 | // SERVO_STATUS.in_pulse = 0;
|
||
57 | // SERVO_STATUS.ms = 0;
|
||
58 | // OCR3B += SERVO_MS; //NOTE--this might mean the negative width is a function of max pos width
|
||
59 | // TCNT3 = 0;
|
||
60 | servo_setup_pulse(); |
||
61 | } |
||
62 | |||
63 | } |
||
64 | else { //we are between pulses |
||
65 | ++SERVO_STATUS.ms; //increment ms counter
|
||
66 | |||
67 | if (SERVO_STATUS.ms >= SERVO_PERIOD) { //see how many ms have passed |
||
68 | servo_setup_pulse(); //Seems like we could do this while not on the clock, but who knows
|
||
69 | } |
||
70 | else {
|
||
71 | OCR3B = SERVO_MS; //go another ms
|
||
72 | TCNT3 = 0;
|
||
73 | |||
74 | } |
||
75 | } |
||
76 | } |
||
77 | |||
78 | |||
79 | |||
80 | void servo_init(void) { |
||
81 | int i;
|
||
82 | uint8_t mysreg; |
||
83 | |||
84 | SERVODDR1 |= SERVOMASK1; //all outputs
|
||
85 | SERVODDR2 |= SERVOMASK2; |
||
86 | |||
87 | mysreg=SREG; |
||
88 | cli(); //turn off interrupts for now
|
||
89 | |||
90 | //init everything
|
||
91 | |||
92 | for (i = 0; i < SERVO_COUNT; ++i) { |
||
93 | SERVO_STATUS.servos[i].num = i; |
||
94 | SERVO_STATUS.servos[i].angle = 127;
|
||
95 | SERVO_STATUS.servo_angles[i] = 127;
|
||
96 | } |
||
97 | |||
98 | SERVO_STATUS.ms = 0;
|
||
99 | SERVO_STATUS.new_angles = 1;
|
||
100 | SERVO_STATUS.in_pulse = 0;
|
||
101 | SERVO_STATUS.change_count = 0;
|
||
102 | |||
103 | //init timer3
|
||
104 | TCCR3A = 0;
|
||
105 | TCCR3B = _BV(CS31); //prescale = 8
|
||
106 | TCCR3C=0;
|
||
107 | TCNT3=0;
|
||
108 | ETIMSK |= _BV(OCIE3B); |
||
109 | OCR3B = SERVO_MS; |
||
110 | |||
111 | SREG=mysreg; |
||
112 | } |
||
113 | |||
114 | void servo_set_angle(int servo, int angle) { |
||
115 | uint8_t mysreg; |
||
116 | if(servo>=8){ //2nd port flipped (on avrxbee at least) |
||
117 | servo= 23-servo;
|
||
118 | } |
||
119 | if (SERVO_STATUS.servo_angles[servo] != angle) {
|
||
120 | mysreg=SREG; |
||
121 | cli(); |
||
122 | SERVO_STATUS.servo_angles[servo] = angle; |
||
123 | SERVO_STATUS.new_angles = 1;
|
||
124 | SREG=mysreg; |
||
125 | } |
||
126 | } |
||
127 | |||
128 | |||
129 | void servo_sort(void) { |
||
130 | int done = 0, i; |
||
131 | |||
132 | while (! done) {
|
||
133 | done = 1;
|
||
134 | |||
135 | for (i = 0; i < SERVO_COUNT - 1; ++i) { //loop through all |
||
136 | |||
137 | //if they are out of order, swap them
|
||
138 | if (SERVO_STATUS.servos[i].angle > SERVO_STATUS.servos[i+1].angle) { |
||
139 | SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;
|
||
140 | SERVO_STATUS.servos[i+1].angle ^= SERVO_STATUS.servos[i].angle;
|
||
141 | SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;
|
||
142 | |||
143 | SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;
|
||
144 | SERVO_STATUS.servos[i+1].num ^= SERVO_STATUS.servos[i].num;
|
||
145 | SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;
|
||
146 | |||
147 | done = 0;
|
||
148 | } |
||
149 | } |
||
150 | } |
||
151 | } |
||
152 | |||
153 | void servo_setup_pulse(void) { |
||
154 | int i;
|
||
155 | uint16_t my_port; |
||
156 | uint16_t sum = 0;
|
||
157 | uint16_t split_time; |
||
158 | |||
159 | my_port = 0xffff; //all on |
||
160 | |||
161 | if (SERVO_STATUS.new_angles) {
|
||
162 | |||
163 | SERVO_STATUS.change_count = 0;
|
||
164 | |||
165 | for (i = 0; i < SERVO_COUNT; ++i) { |
||
166 | SERVO_STATUS.servos[i].angle = SERVO_STATUS.servo_angles[SERVO_STATUS.servos[i].num]; |
||
167 | } |
||
168 | |||
169 | servo_sort(); |
||
170 | SERVO_STATUS.new_angles = 0;
|
||
171 | |||
172 | for (i = 0; i < SERVO_COUNT; ++i) { |
||
173 | split_time = /*SERVO_MIN + ((((((long int)(SERVO_MAX-SERVO_MIN)) >> 3) ) *
|
||
174 | SERVO_STATUS.servos[i].angle) >> 5) */SERVO_STATUS.servos[i].angle - sum;
|
||
175 | my_port &= ~_BV(SERVO_STATUS.servos[i].num); |
||
176 | for (; i < SERVO_COUNT - 1 && SERVO_STATUS.servos[i].angle == SERVO_STATUS.servos[i+1].angle; ++i) { |
||
177 | my_port &= ~_BV(SERVO_STATUS.servos[i+1].num);
|
||
178 | } |
||
179 | |||
180 | SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port; |
||
181 | servo_set_next_interrupt(split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]); |
||
182 | |||
183 | ++SERVO_STATUS.change_count; |
||
184 | |||
185 | sum += split_time; |
||
186 | } |
||
187 | |||
188 | SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port; |
||
189 | servo_set_next_interrupt(SERVO_RESET - split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]); |
||
190 | |||
191 | ++SERVO_STATUS.change_count; |
||
192 | |||
193 | } |
||
194 | |||
195 | |||
196 | |||
197 | SERVO_STATUS.current_servo = 0;
|
||
198 | SERVO_STATUS.in_pulse = 1;
|
||
199 | |||
200 | |||
201 | SERVOPORT1 |= SERVOMASK1; |
||
202 | SERVOPORT2 |= SERVOMASK2; |
||
203 | |||
204 | OCR3B = SERVO_STATUS.changes[0].split_time_period;
|
||
205 | TCNT3=0;
|
||
206 | } |
||
207 | |||
208 | void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn) { |
||
209 | //this function is a holdover from when we used a worse clock.
|
||
210 | |||
211 | (*sn).split_time_period = ticks; |
||
212 | |||
213 | } |