929ff1371ebf69adfce4ceaac0da19c3c4de03e2
[protos/rc_servos.git] / main.c
1 #include <aversive.h>
2 #include <aversive/wait.h>
3
4 struct servo {
5         uint8_t bit;
6         uint16_t command;
7 };
8
9 static struct servo servo_table[] = {
10         {
11                 .bit = 2,
12                 .command = 300,
13         },
14         {
15                 .bit = 3,
16                 .command = 700,
17         },
18         {
19                 .bit = 4,
20                 .command = 512,
21         },
22         {
23                 .bit = 5,
24                 .command = 512,
25         },
26         {
27                 .bit = 6,
28                 .command = 512,
29         },
30         {
31                 .bit = 7,
32                 .command = 512,
33         },
34 };
35 #define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
36
37 static uint8_t bypass;
38 static volatile uint8_t done;
39 static uint8_t portval;
40 static uint8_t rxidx;
41
42 static uint8_t icp_idx = NB_SERVO;
43 static uint16_t icp_servos[NB_SERVO];
44 static uint16_t icp_prev;
45
46 static uint8_t spi_out_idx; /* current byte beeing sent */
47
48 #define BYPASS_ENABLE 14
49 #define BYPASS_DISABLE 15
50
51 #define LED_ON() do { PORTB |= 0x02; } while(0)
52 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
53
54 /*
55  * SPI protocol:
56  *
57  * A command is stored on 2 bytes (except command 0). The first byte
58  * has its most significant bit to 0, and the second one to 1. The
59  * first received byte contains the command number, and the msb of the
60  * servo value. The second byte contains the lsb of the servo value.
61  *
62  * Command 0 is only one byte long, it means "I have nothing to say".
63  * Commands 1 to NB_SERVO+1 are to set the value of servo.
64  * Command 14 is to enable bypass mode.
65  * Command 15 is to disable bypass mode.
66  */
67 union byte0 {
68         uint8_t u8;
69         struct {
70                 /* inverted: little endian */
71                 uint8_t val_msb:3;
72                 uint8_t cmd_num:4;
73                 uint8_t zero:1;
74         };
75 };
76
77 union byte1 {
78         uint8_t u8;
79         struct {
80                 /* inverted: little endian */
81                 uint8_t val_lsb:7;
82                 uint8_t one:1;
83         };
84 };
85
86 SIGNAL(TIMER1_COMPA_vect)
87 {
88         PORTD = portval;
89         TIMSK1 &= ~_BV(OCIE1A);
90         done = 1;
91 }
92
93 static void poll_spi(void)
94 {
95         uint8_t c;
96         uint16_t servo;
97         static union byte0 byte0_rx;
98         union byte1 byte1_rx;
99         union byte0 byte0_tx;
100         static union byte1 byte1_tx;
101
102         /* reception complete ? */
103         if (!(SPSR & (1<<SPIF)))
104                 return;
105
106         c = SPDR;
107
108         /* prepare next TX */
109
110         if ((spi_out_idx & 1) == 0) {
111                 servo = icp_servos[spi_out_idx >> 1];
112                 byte0_tx.val_msb = servo >> 7;
113                 byte0_tx.cmd_num = (spi_out_idx >> 1) + 1;
114                 byte0_tx.zero = 0;
115                 byte1_tx.val_lsb = servo & 0x7f;
116                 byte1_tx.one = 1;
117                 SPDR = byte0_tx.u8;
118         }
119         else {
120                 SPDR = byte1_tx.u8;
121         }
122         spi_out_idx ++;
123         if (spi_out_idx >= NB_SERVO)
124                 spi_out_idx = 0;
125
126         /* RX */
127
128         if ((rxidx == 0) && (c & 0x80)) {
129                 rxidx = 0;
130                 return; /* drop */
131         }
132         if ((rxidx == 1) && ((c & 0x80) == 0)) {
133                 rxidx = 0;
134                 return; /* drop */
135         }
136
137         if (rxidx == 0) {
138                 byte0_rx.u8 = c;
139
140                 /* command num 0 is ignored */
141                 if (byte0_rx.cmd_num == 0)
142                         return;
143         }
144         else {
145                 uint16_t val;
146
147                 byte1_rx.u8 = c;
148
149                 /* process command */
150
151                 if (byte0_rx.cmd_num < NB_SERVO+1) {
152                         val = (uint16_t)byte0_rx.val_msb << 7;
153                         val += byte1_rx.val_lsb;
154                         servo_table[byte0_rx.cmd_num-1].command = val;
155                 }
156                 else if (byte0_rx.cmd_num == BYPASS_ENABLE) {
157                         bypass = 1;
158                 }
159                 else if (byte0_rx.cmd_num == BYPASS_DISABLE) {
160                         bypass = 0;
161                 }
162         }
163
164         rxidx ^= 1;
165 }
166
167 static void poll_input_capture(void)
168 {
169         uint16_t icp, diff;
170         uint8_t rising;
171
172         /* no new sample, return */
173         if ((TIFR1 & _BV(ICF1)) == 0)
174                 return;
175
176         sei();
177         icp = ICR1;
178         cli();
179
180         rising = TCCR1B & _BV(ICES1);
181
182         /* change the edge type */
183         TCCR1B ^= _BV(ICES1);
184
185         /* clear the flag */
186         TIFR1 = TIFR1 | _BV(ICF1);
187
188         diff = icp - icp_prev;
189         icp_prev = icp;
190
191         /* a rising edge with at least 2ms of state 0 means that we
192          * get the first servo */
193         if (rising == 1 && diff > 2000) {
194                 icp_idx = 0;
195                 return;
196         }
197
198         /* get the value for the servo */
199         if (rising == 0 && icp_idx < NB_SERVO) {
200                 if (diff < 1000)
201                         icp_servos[icp_idx] = 0;
202                 else
203                         icp_servos[icp_idx] = diff - 1000;
204                 icp_idx++;
205         }
206 }
207
208 static void poll(void)
209 {
210         poll_spi();
211         poll_input_capture();
212 }
213
214 static void load_timer_at(uint16_t t)
215 {
216         OCR1A = t;
217         TIMSK1 |= _BV(OCIE1A);
218 }
219
220 static void do_one_servo(struct servo *s)
221 {
222         uint16_t t;
223
224         /* set bit */
225         done = 0;
226         //portval = PORTC | (1 << s->bit);
227         portval = (1 << s->bit);
228         t = TCNT1;
229         load_timer_at(t + 20);
230         while (done == 0)
231                 poll();
232
233         /* reset bit */
234         done = 0;
235         portval = 0;
236         //portval = PORTC & (~(1 << s->bit));
237         load_timer_at(t + 20 + 1000 + s->command);
238         while (done == 0)
239                 poll();
240 }
241
242 int main(void)
243 {
244         uint8_t i;
245         uint8_t t, diff;
246         uint8_t tmp;
247
248         /* use pull-up for inputs */
249         PORTC |= 0x3f;
250
251         /* LED */
252         DDRB = 0x02;
253
254 #if 0 /* LED debug */
255         while (1) {
256                 LED_ON();
257                 wait_ms(100);
258                 LED_OFF();
259                 wait_ms(100);
260         }
261 #endif
262
263         /* servo outputs PD2-PD7 */
264         DDRD = 0xfc;
265
266         /* start timer1 at clk/8 (1Mhz), enable noise canceler on
267          * input capture, capture rising edge */
268         TCNT1 = 0;
269         TCCR1B = _BV(CS11) | _BV(ICNC1) | _BV(ICES1);
270
271         /* start timer0 at clk/1024 (~8Khz) */
272         TCNT0 = 0;
273         TCCR0B = _BV(CS02) | _BV(CS00);
274
275         /* enable spi (set MISO as output) */
276         SPCR = _BV(SPE);
277         SPDR = 0;
278         DDRB |= _BV(4);
279
280         sei();
281
282         bypass = 0;
283         while (1) {
284                 t = TCNT0;
285                 for (i = 0; i < NB_SERVO; i++) {
286                         do_one_servo(&servo_table[i]);
287                 }
288                 /* wait 20 ms */
289                 while (1) {
290                         diff = TCNT0 - t;
291                         if (diff >= 160)
292                                 break;
293                         poll();
294                 }
295                 /* bypass mode */
296                 if (bypass == 1) {
297                         LED_ON();
298
299                         while (bypass == 1) {
300                                 tmp = PINC;
301                                 tmp &= 0x3f;
302                                 tmp <<= 2;
303                                 PORTD = tmp;
304                                 poll();
305                         }
306                         LED_OFF();
307                 }
308         }
309
310         return 0;
311 }