2 #include <aversive/wait.h>
9 static struct servo servo_table[] = {
35 #define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
37 static uint8_t bypass;
38 static volatile uint8_t done;
39 static uint8_t portval;
42 #define BYPASS_ENABLE 14
43 #define BYPASS_DISABLE 15
45 #define LED_ON() do { PORTB |= 0x02; } while(0)
46 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
51 * A command is stored on 2 bytes. The first one has its msb to 0, and the
52 * second one to 1. The first received byte contains the command number, and the
53 * msb of the servo value. The second byte contains the lsb of the servo value.
55 * Commands 0 to NB_SERVO are to set the value of servo.
56 * Command 14 is to enable bypass mode.
57 * Command 15 is to disable bypass mode.
62 /* inverted: little endian */
72 /* inverted: little endian */
78 SIGNAL(TIMER1_COMPA_vect)
81 TIMSK1 &= ~_BV(OCIE1A);
85 static void poll_spi(void)
89 /* reception complete ? */
90 if (!(SPSR & (1<<SPIF)))
94 if ((rxidx == 0) && (c & 0x80)) {
98 if ((rxidx == 1) && ((c & 0x80) == 0)) {
111 /* process command */
113 if (byte0.cmd_num < NB_SERVO) {
114 val = (uint16_t)byte0.val_msb << 7;
115 val += byte1.val_lsb;
116 servo_table[byte0.cmd_num].command = val;
118 else if (byte0.cmd_num == BYPASS_ENABLE) {
121 else if (byte0.cmd_num == BYPASS_DISABLE) {
129 static void load_timer_at(uint16_t t)
132 TIMSK1 |= _BV(OCIE1A);
135 static void do_one_servo(struct servo *s)
141 //portval = PORTC | (1 << s->bit);
142 portval = (1 << s->bit);
144 load_timer_at(t + 20);
151 //portval = PORTC & (~(1 << s->bit));
152 load_timer_at(t + 20 + 1000 + s->command);
163 /* use pull-up for inputs */
169 #if 0 /* LED debug */
178 /* servo outputs PD2-PD7 */
181 /* start timer1 at clk/8 (1Mhz) */
185 /* start timer0 at clk/1024 (~8Khz) */
187 TCCR0B = _BV(CS02) | _BV(CS00);
189 /* enable spi (don't set unused MISO as output) */
197 for (i = 0; i < NB_SERVO; i++) {
198 do_one_servo(&servo_table[i]);
211 while (bypass == 1) {