8 static struct servo servo_table[] = {
30 #define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
32 static volatile uint8_t bypass;
33 static volatile uint8_t done;
34 static volatile uint8_t portval;
35 static volatile uint8_t rxidx;
37 #define BYPASS_ENABLE 14
38 #define BYPASS_DISABLE 15
43 * A command is stored on 2 bytes. The first one has its msb to 0, and the
44 * second one to 1. The first received byte contains the command number, and the
45 * msb of the servo value. The second byte contains the lsb of the servo value.
47 * Commands 0 to NB_SERVO are to set the value of servo.
48 * Command 14 is to enable bypass mode.
49 * Command 15 is to disable bypass mode.
51 static volatile union {
54 /* inverted: little endian */
61 static volatile union {
64 /* inverted: little endian */
70 SIGNAL(TIMER1_COMPA_vect)
73 TIMSK1 &= ~_BV(OCIE1A);
77 static void poll_spi(void)
81 /* reception complete ? */
82 if (!(SPSR & (1<<SPIF)))
86 if ((rxidx == 0) && (c & 0x80)) {
90 if ((rxidx == 1) && ((c & 0x80) == 0)) {
103 /* process command */
105 if (byte0.cmd_num < NB_SERVO) {
106 val = (uint16_t)byte0.val_msb << 7;
107 val += byte1.val_lsb;
108 servo_table[byte0.cmd_num].command = val;
110 else if (byte0.cmd_num == BYPASS_ENABLE) {
113 else if (byte0.cmd_num == BYPASS_DISABLE) {
121 static void load_timer_at(uint16_t t)
124 TIMSK1 |= _BV(OCIE1A);
127 static void do_one_servo(struct servo *s)
133 //portval = PORTC | (1 << s->bit);
134 portval = (1 << s->bit);
136 load_timer_at(t + 150);
143 //portval = PORTC & (~(1 << s->bit));
144 load_timer_at(t + 150 + 8000 + s->command * 8);
161 /* start timer1 at clk/1 (8Mhz) */
165 /* start timer0 at clk/1024 (~8Khz) */
167 TCCR0B = _BV(CS02) | _BV(CS00);
169 /* enable spi (don't set unused MISO as output) */
177 for (i = 0; i < NB_SERVO; i++) {
178 do_one_servo(&servo_table[i]);
188 while (bypass == 1) {
189 PORTD = ((PINC & 0x0f) << 3) | ((PINB & 0x01) << 7);