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 * 2)
 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
 171         /* no new sample, return */
 172         if ((TIFR1 & _BV(ICF1)) == 0)
 173                 return;
 174
 175         cli();
 176         icp = ICR1;
 177         sei();
 178
 179         /* clear the flag by writing a one */
 180         TIFR1 = TIFR1 | _BV(ICF1);
 181
 182         diff = icp - icp_prev;
 183         icp_prev = icp;
 184
 185         /* a rising edge with at least 2ms of state 0 means that we
 186          * get the first servo */
 187         if (diff > 3000) {
 188                 icp_idx = 0;
 189                 return;
 190         }
 191
 192         /* get the value for the servo */
 193         if (icp_idx < NB_SERVO) {
 194                 if (diff < 1000)
 195                         icp_servos[icp_idx] = 0;
 196                 else if (diff > 2023)
 197                         icp_servos[icp_idx] = 1023;
 198                 else
 199                         icp_servos[icp_idx] = diff - 1000;
 200                 icp_idx++;
 201         }
 202 }
 203
 204 static void poll(void)
 205 {
 206         poll_spi();
 207         poll_input_capture();
 208 }
 209
 210 static void load_timer_at(uint16_t t)
 211 {
 212         OCR1A = t;
 213         TIMSK1 |= _BV(OCIE1A);
 214 }
 215
 216 static void do_one_servo(struct servo *s)
 217 {
 218         uint16_t t;
 219
 220         /* set bit */
 221         done = 0;
 222         //portval = PORTC | (1 << s->bit);
 223         portval = (1 << s->bit);
 224         t = TCNT1;
 225         load_timer_at(t + 20);
 226         while (done == 0)
 227                 poll();
 228
 229         /* reset bit */
 230         done = 0;
 231         portval = 0;
 232         //portval = PORTC & (~(1 << s->bit));
 233         load_timer_at(t + 20 + 1000 + s->command);
 234         while (done == 0)
 235                 poll();
 236 }
 237
 238 int main(void)
 239 {
 240         uint8_t i;
 241         uint8_t t, diff;
 242         uint8_t tmp;
 243         uint8_t cnt = 10;
 244
 245         /* use pull-up for inputs */
 246         PORTC |= 0x3f;
 247
 248         /* LED */
 249         DDRB = 0x02;
 250
 251         while (cnt > 0) {
 252 #if 1 /* disable for LED debug only */
 253                 cnt--;
 254 #endif
 255                 LED_ON();
 256                 wait_ms(100);
 257                 LED_OFF();
 258                 wait_ms(100);
 259         }
 260
 261         /* servo outputs PD2-PD7 */
 262         DDRD = 0xfc;
 263
 264         /* start timer1 at clk/8 (1Mhz), enable noise canceler on
 265          * input capture, capture rising edge */
 266         TCNT1 = 0;
 267         TCCR1B = _BV(CS11) | _BV(ICNC1) | _BV(ICES1);
 268
 269         /* start timer0 at clk/1024 (~8Khz) */
 270         TCNT0 = 0;
 271         TCCR0B = _BV(CS02) | _BV(CS00);
 272
 273         /* enable spi (set MISO as output) */
 274         SPCR = _BV(SPE);
 275         SPDR = 0;
 276         DDRB |= _BV(4);
 277
 278         sei();
 279
 280         bypass = 0;
 281         while (1) {
 282                 t = TCNT0;
 283                 for (i = 0; i < NB_SERVO; i++) {
 284                         do_one_servo(&servo_table[i]);
 285                 }
 286                 /* wait 20 ms */
 287                 while (1) {
 288                         diff = TCNT0 - t;
 289                         if (diff >= 160)
 290                                 break;
 291                         poll();
 292                 }
 293                 /* bypass mode */
 294                 if (bypass == 1) {
 295                         LED_ON();
 296
 297                         while (bypass == 1) {
 298                                 tmp = PINC;
 299                                 tmp &= 0x3f;
 300                                 tmp <<= 2;
 301                                 PORTD = tmp;
 302                                 poll();
 303                         }
 304                         LED_OFF();
 305                 }
 306         }
 307
 308         return 0;
 309 }