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 }