spi_servo.c

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