main.c

   1 #include <aversive.h>
   2
   3 struct servo {
   4         uint8_t bit;
   5         uint16_t command;
   6 };
   7
   8 static struct servo servo_table[] = {
   9         {
  10                 .bit = 0,
  11                 .command = 0,
  12         },
  13         {
  14                 .bit = 1,
  15                 .command = 512,
  16         },
  17         {
  18                 .bit = 2,
  19                 .command = 1023,
  20         },
  21 };
  22 #define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
  23
  24 static volatile uint8_t bypass;
  25 static volatile uint8_t done;
  26 static volatile uint8_t portval;
  27 static volatile uint8_t rxidx;
  28
  29 #define BYPASS_ENABLE 14
  30 #define BYPASS_DISABLE 15
  31
  32 /*
  33  * SPI protocol:
  34  *
  35  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  36  * second one to 1. The first received byte contains the command number, and the
  37  * msb of the servo value. The second byte contains the lsb of the servo value.
  38  *
  39  * Commands 0 to NB_SERVO are to set the value of servo.
  40  * Command 14 is to enable bypass mode.
  41  * Command 15 is to disable bypass mode.
  42  */
  43 static volatile union {
  44         uint8_t u8;
  45         struct {
  46                 uint8_t zero:1;
  47                 uint8_t cmd_num:4;
  48                 uint8_t val_msb:3;
  49         };
  50 } byte0;
  51
  52 static volatile union {
  53         uint8_t u8;
  54         struct {
  55                 uint8_t one:1;
  56                 uint8_t val_lsb:7;
  57         };
  58 } byte1;
  59
  60 SIGNAL(TIMER1_COMPA_vect)
  61 {
  62         PORTC = portval;
  63         TIMSK1 &= ~_BV(OCIE1A);
  64         done = 1;
  65 }
  66
  67 static void poll_spi(void)
  68 {
  69         uint8_t c;
  70
  71         /* reception complete ? */
  72         if (!(SPSR & (1<<SPIF)))
  73                 return;
  74
  75         c = SPDR;
  76         if ((rxidx == 0) && (c & 0x80)) {
  77                 rxidx = 0;
  78                 return; /* drop */
  79         }
  80         if ((rxidx == 1) && ((c & 0x80) == 0)) {
  81                 rxidx = 0;
  82                 return; /* drop */
  83         }
  84
  85         if (rxidx == 0) {
  86                 byte0.u8 = c;
  87         }
  88         else {
  89                 uint16_t val;
  90
  91                 byte1.u8 = c;
  92
  93                 /* process command */
  94
  95                 if (byte0.cmd_num < NB_SERVO) {
  96                         val = (uint16_t)byte0.val_msb << 7;
  97                         val += byte1.val_lsb;
  98                         servo_table[byte0.cmd_num].command = val;
  99                 }
 100                 else if (byte0.cmd_num == BYPASS_ENABLE) {
 101                         bypass = 1;
 102                 }
 103                 else if (byte0.cmd_num == BYPASS_DISABLE) {
 104                         bypass = 0;
 105                 }
 106         }
 107
 108         rxidx ^= 1;
 109 }
 110
 111 static void load_timer_at(uint16_t t)
 112 {
 113         OCR1A = t;
 114         TIMSK1 |= _BV(OCIE1A);
 115 }
 116
 117 static void do_one_servo(struct servo *s)
 118 {
 119         uint16_t t;
 120
 121         /* set bit */
 122         done = 0;
 123         //portval = PORTC | (1 << s->bit);
 124         portval = (1 << s->bit);
 125         t = TCNT1;
 126         load_timer_at(t + 150);
 127         while (done == 0)
 128                 poll_spi();
 129
 130         /* reset bit */
 131         done = 0;
 132         portval = 0;
 133         //portval = PORTC & (~(1 << s->bit));
 134         load_timer_at(t + 150 + 8000 + s->command * 8);
 135         while (done == 0)
 136                 poll_spi();
 137 }
 138
 139 int main(void)
 140 {
 141         uint8_t i;
 142         uint8_t t, diff;
 143
 144         /* LED */
 145         DDRB = 0x20;
 146
 147         /* servo outputs */
 148         DDRC = 0x7;
 149
 150         /* start timer1 at clk/1 (8Mhz) */
 151         TCNT1 = 0;
 152         TCCR1B = _BV(CS10);
 153
 154         /* start timer0 at clk/1024 (~8Khz) */
 155         TCNT0 = 0;
 156         TCCR0B = _BV(CS02) | _BV(CS00);
 157
 158         /* enable spi (don't set unused MISO as output) */
 159         SPCR = _BV(SPE);
 160
 161         sei();
 162
 163         bypass = 0;
 164         while (1) {
 165                 t = TCNT0;
 166                 for (i = 0; i < NB_SERVO; i++) {
 167                         do_one_servo(&servo_table[i]);
 168                 }
 169                 /* wait 20 ms */
 170                 while (1) {
 171                         diff = TCNT0 - t;
 172                         if (diff >= 160)
 173                                 break;
 174                         poll_spi();
 175                 }
 176                 /* bypass mode */
 177                 while (bypass == 1) {
 178                         PORTC = PORTB;
 179                 }
 180         }
 181
 182         return 0;
 183 }