spi_servo.c

   1 #include <aversive.h>
   2 #include <aversive/wait.h>
   3 #include <stdint.h>
   4
   5 #include "spi_servo.h"
   6
   7 #define BYPASS_ENABLE 14
   8 #define BYPASS_DISABLE 15
   9
  10 /*
  11  * SPI protocol:
  12  *
  13  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  14  * second one to 1. The first received byte contains the command number, and the
  15  * msb of the servo value. The second byte contains the lsb of the servo value.
  16  *
  17  * Commands 0 to NB_SERVO are to set the value of servo.
  18  * Command 14 is to enable bypass mode.
  19  * Command 15 is to disable bypass mode.
  20  */
  21 static volatile union {
  22         uint8_t u8;
  23         struct {
  24                 /* inverted: little endian */
  25                 uint8_t val_msb:3;
  26                 uint8_t cmd_num:4;
  27                 uint8_t zero:1;
  28         };
  29 } byte0;
  30
  31 static volatile union {
  32         uint8_t u8;
  33         struct {
  34                 /* inverted: little endian */
  35                 uint8_t val_lsb:7;
  36                 uint8_t one:1;
  37         };
  38 } byte1;
  39
  40 void spi_servo_init(void)
  41 {
  42         /* real SS ! */
  43         DDRK = 0x2;
  44         /* SCK, SS & MOSI */
  45         DDRB = 0x7;
  46
  47         /* remove power reduction on spi */
  48         PRR0 &= ~(1 << PRSPI);
  49
  50         /* Enable SPI, Master, set clock rate fck/64 */
  51         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
  52
  53         PORTK |= (1<<1);
  54
  55         spi_servo_set(BYPASS_DISABLE, 0);
  56 }
  57
  58 void spi_servo_set(uint8_t num, uint16_t val)
  59 {
  60         byte0.val_msb = val >> 7;
  61         byte0.cmd_num = num;
  62         byte0.zero = 0;
  63         byte1.one = 1;
  64         byte1.val_lsb = val;
  65
  66         PORTK &= ~(1<<1);
  67         SPDR = byte0.u8;
  68         /* Wait for transmission complete */
  69         while(!(SPSR & (1<<SPIF)));
  70         PORTK |= (1<<1);
  71
  72         _delay_loop_1(5);
  73         PORTK &= ~(1<<1);
  74
  75         SPDR = byte1.u8;
  76         /* Wait for transmission complete */
  77         while(!(SPSR & (1<<SPIF)));
  78         PORTK |= (1<<1);
  79 }
  80
  81 void spi_servo_bypass(uint8_t enable)
  82 {
  83         if (enable)
  84                 spi_servo_set(BYPASS_ENABLE, 0);
  85         else
  86                 spi_servo_set(BYPASS_DISABLE, 0);
  87 }