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 #define BYPASS_ENABLE 14
  43 #define BYPASS_DISABLE 15
  44
  45 #define LED_ON() do { PORTB |= 0x02; } while(0)
  46 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
  47
  48 /*
  49  * SPI protocol:
  50  *
  51  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  52  * second one to 1. The first received byte contains the command number, and the
  53  * msb of the servo value. The second byte contains the lsb of the servo value.
  54  *
  55  * Commands 0 to NB_SERVO are to set the value of servo.
  56  * Command 14 is to enable bypass mode.
  57  * Command 15 is to disable bypass mode.
  58  */
  59 static union {
  60         uint8_t u8;
  61         struct {
  62                 /* inverted: little endian */
  63                 uint8_t val_msb:3;
  64                 uint8_t cmd_num:4;
  65                 uint8_t zero:1;
  66         };
  67 } byte0;
  68
  69 static union {
  70         uint8_t u8;
  71         struct {
  72                 /* inverted: little endian */
  73                 uint8_t val_lsb:7;
  74                 uint8_t one:1;
  75         };
  76 } byte1;
  77
  78 SIGNAL(TIMER1_COMPA_vect)
  79 {
  80         PORTD = portval;
  81         TIMSK1 &= ~_BV(OCIE1A);
  82         done = 1;
  83 }
  84
  85 static void poll_spi(void)
  86 {
  87         uint8_t c;
  88
  89         /* reception complete ? */
  90         if (!(SPSR & (1<<SPIF)))
  91                 return;
  92
  93         c = SPDR;
  94         if ((rxidx == 0) && (c & 0x80)) {
  95                 rxidx = 0;
  96                 return; /* drop */
  97         }
  98         if ((rxidx == 1) && ((c & 0x80) == 0)) {
  99                 rxidx = 0;
 100                 return; /* drop */
 101         }
 102
 103         if (rxidx == 0) {
 104                 byte0.u8 = c;
 105         }
 106         else {
 107                 uint16_t val;
 108
 109                 byte1.u8 = c;
 110
 111                 /* process command */
 112
 113                 if (byte0.cmd_num < NB_SERVO) {
 114                         val = (uint16_t)byte0.val_msb << 7;
 115                         val += byte1.val_lsb;
 116                         servo_table[byte0.cmd_num].command = val;
 117                 }
 118                 else if (byte0.cmd_num == BYPASS_ENABLE) {
 119                         bypass = 1;
 120                 }
 121                 else if (byte0.cmd_num == BYPASS_DISABLE) {
 122                         bypass = 0;
 123                 }
 124         }
 125
 126         rxidx ^= 1;
 127 }
 128
 129 static void load_timer_at(uint16_t t)
 130 {
 131         OCR1A = t;
 132         TIMSK1 |= _BV(OCIE1A);
 133 }
 134
 135 static void do_one_servo(struct servo *s)
 136 {
 137         uint16_t t;
 138
 139         /* set bit */
 140         done = 0;
 141         //portval = PORTC | (1 << s->bit);
 142         portval = (1 << s->bit);
 143         t = TCNT1;
 144         load_timer_at(t + 20);
 145         while (done == 0)
 146                 poll_spi();
 147
 148         /* reset bit */
 149         done = 0;
 150         portval = 0;
 151         //portval = PORTC & (~(1 << s->bit));
 152         load_timer_at(t + 20 + 1000 + s->command);
 153         while (done == 0)
 154                 poll_spi();
 155 }
 156
 157 int main(void)
 158 {
 159         uint8_t i;
 160         uint8_t t, diff;
 161         uint8_t tmp;
 162
 163         /* use pull-up for inputs */
 164         PORTC |= 0x3f;
 165
 166         /* LED */
 167         DDRB = 0x02;
 168
 169 #if 0 /* LED debug */
 170         while (1) {
 171                 LED_ON();
 172                 wait_ms(100);
 173                 LED_OFF();
 174                 wait_ms(100);
 175         }
 176 #endif
 177
 178         /* servo outputs PD2-PD7 */
 179         DDRD = 0xfc;
 180
 181         /* start timer1 at clk/8 (1Mhz) */
 182         TCNT1 = 0;
 183         TCCR1B = _BV(CS11);
 184
 185         /* start timer0 at clk/1024 (~8Khz) */
 186         TCNT0 = 0;
 187         TCCR0B = _BV(CS02) | _BV(CS00);
 188
 189         /* enable spi (don't set unused MISO as output) */
 190         SPCR = _BV(SPE);
 191
 192         sei();
 193
 194         bypass = 0;
 195         while (1) {
 196                 t = TCNT0;
 197                 for (i = 0; i < NB_SERVO; i++) {
 198                         do_one_servo(&servo_table[i]);
 199                 }
 200                 /* wait 20 ms */
 201                 while (1) {
 202                         diff = TCNT0 - t;
 203                         if (diff >= 160)
 204                                 break;
 205                         poll_spi();
 206                 }
 207                 /* bypass mode */
 208                 if (bypass == 1) {
 209                         LED_ON();
 210
 211                         while (bypass == 1) {
 212                                 tmp = PINC;
 213                                 tmp &= 0x3f;
 214                                 tmp <<= 2;
 215                                 PORTD = tmp;
 216                                 poll_spi();
 217                         }
 218                         LED_OFF();
 219                 }
 220         }
 221
 222         return 0;
 223 }