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