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