main.c

   1 /*
   2 avrdude -p atmega168p -P usb -c avrispmkii -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xf9:m
   3 -> it failed but I answered y, then make reset and it was ok
   4 */
   5
   6 #include <aversive.h>
   7 #include <aversive/wait.h>
   8
   9 struct servo {
  10         uint8_t bit;
  11         uint16_t command;
  12 };
  13
  14 static struct servo servo_table[] = {
  15         {
  16                 .bit = 2,
  17                 .command = 300,
  18         },
  19         {
  20                 .bit = 3,
  21                 .command = 700,
  22         },
  23         {
  24                 .bit = 4,
  25                 .command = 512,
  26         },
  27         {
  28                 .bit = 5,
  29                 .command = 512,
  30         },
  31         {
  32                 .bit = 6,
  33                 .command = 512,
  34         },
  35         {
  36                 .bit = 7,
  37                 .command = 512,
  38         },
  39 };
  40 #define N_SERVO (sizeof(servo_table)/sizeof(*servo_table))
  41
  42 /* we use the first servo for PPM output if enabled */
  43 #define PPM (0)
  44
  45 static uint8_t bypass;
  46 static uint8_t ppm_enabled;
  47 static volatile uint8_t done;
  48 static uint8_t portval;
  49 static uint8_t rxidx;
  50
  51 static uint8_t icp_idx = N_SERVO;
  52 static uint16_t icp_servos[N_SERVO];
  53 static uint16_t icp_prev;
  54
  55 static uint8_t spi_out_idx; /* current byte beeing sent */
  56
  57 #define PPM_BIT    0x01
  58 #define BYPASS_BIT 0x02
  59
  60 #define LED_ON() do { PORTB |= 0x02; } while(0)
  61 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
  62
  63 /*
  64  * SPI protocol:
  65  *
  66  * A command is stored on 2 bytes (except command 0). The first byte
  67  * has its most significant bit to 0, and the second one to 1. The
  68  * first received byte contains the command number, and the msb of the
  69  * servo value. The second byte contains the lsb of the servo value.
  70  *
  71  * Command 0 is only one byte long, it means "I have nothing to say".
  72  * Commands 1 to N_SERVO (included) are to set the value of servo.
  73  * Command N_SERVO+1 is:
  74  * - to enable/disable ppm generation in place of first servo.
  75  * - to enable/disable bypass mode
  76  */
  77 union byte0 {
  78         uint8_t u8;
  79         struct {
  80                 /* inverted: little endian */
  81                 uint8_t val_msb:3;
  82                 uint8_t cmd_num:4;
  83                 uint8_t zero:1;
  84         };
  85 };
  86
  87 union byte1 {
  88         uint8_t u8;
  89         struct {
  90                 /* inverted: little endian */
  91                 uint8_t val_lsb:7;
  92                 uint8_t one:1;
  93         };
  94 };
  95
  96 SIGNAL(TIMER1_COMPA_vect)
  97 {
  98         PORTD = portval;
  99         TIMSK1 &= ~_BV(OCIE1A);
 100         done = 1;
 101 }
 102
 103 static void poll_spi(void)
 104 {
 105         uint8_t c;
 106         uint16_t servo;
 107         static union byte0 byte0_rx;
 108         union byte1 byte1_rx;
 109         union byte0 byte0_tx;
 110         static union byte1 byte1_tx;
 111
 112         /* reception complete ? */
 113         if (!(SPSR & (1<<SPIF)))
 114                 return;
 115
 116         c = SPDR;
 117
 118         /* prepare next TX */
 119
 120         if ((spi_out_idx & 1) == 0) {
 121                 servo = icp_servos[spi_out_idx >> 1];
 122                 byte0_tx.val_msb = servo >> 7;
 123                 byte0_tx.cmd_num = (spi_out_idx >> 1) + 1;
 124                 byte0_tx.zero = 0;
 125                 byte1_tx.val_lsb = servo & 0x7f;
 126                 byte1_tx.one = 1;
 127                 SPDR = byte0_tx.u8;
 128         }
 129         else {
 130                 SPDR = byte1_tx.u8;
 131         }
 132         spi_out_idx ++;
 133         if (spi_out_idx >= N_SERVO * 2)
 134                 spi_out_idx = 0;
 135
 136         /* RX */
 137
 138         if ((rxidx == 0) && (c & 0x80)) {
 139                 rxidx = 0;
 140                 return; /* drop */
 141         }
 142         if ((rxidx == 1) && ((c & 0x80) == 0)) {
 143                 rxidx = 0;
 144                 return; /* drop */
 145         }
 146
 147         if (rxidx == 0) {
 148                 byte0_rx.u8 = c;
 149
 150                 /* command num 0 is ignored */
 151                 if (byte0_rx.cmd_num == 0)
 152                         return;
 153         }
 154         else {
 155                 uint16_t val;
 156
 157                 byte1_rx.u8 = c;
 158
 159                 /* process command */
 160
 161                 val = (uint16_t)byte0_rx.val_msb << 7;
 162                 val += byte1_rx.val_lsb;
 163
 164                 if (byte0_rx.cmd_num < N_SERVO+1) {
 165                         servo_table[byte0_rx.cmd_num-1].command = val;
 166                 }
 167                 else if (byte0_rx.cmd_num == N_SERVO+1) {
 168                         if (val & PPM_BIT)
 169                                 ppm_enabled = 1;
 170                         else
 171                                 ppm_enabled = 0;
 172                         if (val & BYPASS_BIT)
 173                                 bypass = 1;
 174                         else
 175                                 bypass = 0;
 176                 }
 177         }
 178
 179         rxidx ^= 1;
 180 }
 181
 182 static void poll_input_capture(void)
 183 {
 184         uint16_t icp, diff;
 185
 186         /* no new sample, return */
 187         if ((TIFR1 & _BV(ICF1)) == 0)
 188                 return;
 189
 190         cli();
 191         icp = ICR1;
 192         sei();
 193
 194         /* clear the flag by writing a one */
 195         TIFR1 = TIFR1 | _BV(ICF1);
 196
 197         diff = icp - icp_prev;
 198         icp_prev = icp;
 199
 200         /* a rising edge with at least 2ms of state 0 means that we
 201          * get the first servo */
 202         if (diff > 3000) {
 203                 icp_idx = 0;
 204                 return;
 205         }
 206
 207         /* get the value for the servo */
 208         if (icp_idx < N_SERVO) {
 209                 if (diff < 1000)
 210                         icp_servos[icp_idx] = 0;
 211                 else if (diff > 2023)
 212                         icp_servos[icp_idx] = 1023;
 213                 else
 214                         icp_servos[icp_idx] = diff - 1000;
 215                 icp_idx++;
 216         }
 217 }
 218
 219 static void poll(void)
 220 {
 221         poll_spi();
 222         poll_input_capture();
 223 }
 224
 225 static void load_timer_at(uint16_t t)
 226 {
 227         OCR1A = t;
 228         TIMSK1 |= _BV(OCIE1A);
 229 }
 230
 231 static void do_servos(void)
 232 {
 233         uint8_t i;
 234         uint16_t t, start;
 235
 236         /* skip first servo if ppm is enabled */
 237         if (ppm_enabled)
 238                 i = 1;
 239         else
 240                 i = 0;
 241
 242         t = TCNT1;
 243         start = t + 100;
 244
 245         for (; i < N_SERVO; i++) {
 246
 247                 /* set servo and PPM bit */
 248                 portval = 1 << servo_table[i].bit;
 249                 if (ppm_enabled)
 250                         portval |= (1 << servo_table[PPM].bit);
 251
 252                 done = 0;
 253                 load_timer_at(start);
 254                 while (done == 0)
 255                         poll();
 256
 257                 /* reset PPM bit after 300us */
 258                 portval = 1 << servo_table[i].bit;
 259                 done = 0;
 260                 load_timer_at(start + 300);
 261                 while (done == 0)
 262                         poll();
 263
 264                 start = start + 1000 + servo_table[i].command;
 265         }
 266
 267         /* set PPM bit only for last servo */
 268         portval = 0;
 269         if (ppm_enabled)
 270                 portval |= (1 << servo_table[PPM].bit);
 271
 272         done = 0;
 273         load_timer_at(start);
 274         while (done == 0)
 275                 poll();
 276
 277         /* reset PPM bit after 300us */
 278         portval = 0;
 279         done = 0;
 280         load_timer_at(start + 300);
 281         while (done == 0)
 282                 poll();
 283 }
 284
 285 int main(void)
 286 {
 287         uint8_t t, diff;
 288         uint8_t tmp;
 289         uint8_t cnt = 10;
 290
 291         /* use pull-up for inputs */
 292         PORTC |= 0x3f;
 293
 294         /* LED */
 295         DDRB = 0x02;
 296
 297         while (cnt > 0) {
 298 #if 1 /* disable for LED debug only */
 299                 cnt--;
 300 #endif
 301                 LED_ON();
 302                 wait_ms(100);
 303                 LED_OFF();
 304                 wait_ms(100);
 305         }
 306
 307         /* servo outputs PD2-PD7 */
 308         DDRD = 0xfc;
 309
 310         /* start timer1 at clk/8 (1Mhz), enable noise canceler on
 311          * input capture, capture rising edge */
 312         TCNT1 = 0;
 313         TCCR1B = _BV(CS11) | _BV(ICNC1) | _BV(ICES1);
 314
 315         /* start timer0 at clk/1024 (~8Khz) */
 316         TCNT0 = 0;
 317         TCCR0B = _BV(CS02) | _BV(CS00);
 318
 319         /* enable spi (set MISO as output) */
 320         SPCR = _BV(SPE);
 321         SPDR = 0;
 322         DDRB |= _BV(4);
 323
 324         sei();
 325
 326         bypass = 0;
 327         while (1) {
 328                 t = TCNT0;
 329                 do_servos();
 330                 /* wait 20 ms */
 331                 while (1) {
 332                         diff = TCNT0 - t;
 333                         if (diff >= 160)
 334                                 break;
 335                         poll();
 336                 }
 337                 /* bypass mode */
 338                 if (bypass == 1) {
 339                         LED_ON();
 340
 341                         while (bypass == 1) {
 342                                 tmp = PINC;
 343                                 tmp &= 0x3f;
 344                                 tmp <<= 2;
 345                                 PORTD = tmp;
 346                                 poll();
 347                         }
 348                         LED_OFF();
 349                 }
 350         }
 351
 352         return 0;
 353 }