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