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 volatile uint8_t bypass;
  38 static volatile uint8_t done;
  39 static volatile uint8_t portval;
  40 static volatile 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 volatile 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 volatile 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 + 150);
 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 + 150 + 8000 + s->command * 8);
 153         while (done == 0)
 154                 poll_spi();
 155 }
 156
 157 int main(void)
 158 {
 159         uint8_t i;
 160         uint8_t t, diff;
 161
 162         /* LED */
 163         DDRB = 0x02;
 164
 165 #if 0 /* LED debug */
 166         while (1) {
 167                 LED_ON();
 168                 wait_ms(100);
 169                 LED_OFF();
 170                 wait_ms(100);
 171         }
 172 #endif
 173
 174         /* servo outputs PD2-PD7 */
 175         DDRD = 0xfc;
 176
 177         /* start timer1 at clk/1 (8Mhz) */
 178         TCNT1 = 0;
 179         TCCR1B = _BV(CS10);
 180
 181         /* start timer0 at clk/1024 (~8Khz) */
 182         TCNT0 = 0;
 183         TCCR0B = _BV(CS02) | _BV(CS00);
 184
 185         /* enable spi (don't set unused MISO as output) */
 186         SPCR = _BV(SPE);
 187
 188         sei();
 189
 190         bypass = 0;
 191         while (1) {
 192                 t = TCNT0;
 193                 for (i = 0; i < NB_SERVO; i++) {
 194                         do_one_servo(&servo_table[i]);
 195                 }
 196                 /* wait 20 ms */
 197                 while (1) {
 198                         diff = TCNT0 - t;
 199                         if (diff >= 160)
 200                                 break;
 201                         poll_spi();
 202                 }
 203                 /* bypass mode */
 204                 while (bypass == 1) {
 205                         LED_ON();
 206                         PORTD = (PINC & 0x3f) << 2;
 207                         poll_spi();
 208                 }
 209                 LED_OFF();
 210         }
 211
 212         return 0;
 213 }