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