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 void spi_servo_init(void)
  42 {
  43         /* real SS ! */
  44         DDRK = 0x2;
  45         /* SCK, SS & MOSI */
  46         DDRB = 0x7;
  47
  48         /* remove power reduction on spi */
  49         PRR0 &= ~(1 << PRSPI);
  50
  51         /* Enable SPI, Master, set clock rate fck/64 */
  52         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
  53
  54         PORTK |= (1<<1);
  55
  56         spi_servo_set(BYPASS_DISABLE, 0);
  57 }
  58
  59 void spi_servo_set(uint8_t num, uint16_t val)
  60 {
  61         byte0.val_msb = val >> 7;
  62         byte0.cmd_num = num;
  63         byte0.zero = 0;
  64         byte1.one = 1;
  65         byte1.val_lsb = val;
  66
  67         PORTK &= ~(1<<1);
  68         SPDR = byte0.u8;
  69         /* Wait for transmission complete */
  70         while(!(SPSR & (1<<SPIF)));
  71         PORTK |= (1<<1);
  72
  73         _delay_loop_1(5);
  74         PORTK &= ~(1<<1);
  75
  76         SPDR = byte1.u8;
  77         /* Wait for transmission complete */
  78         while(!(SPSR & (1<<SPIF)));
  79         PORTK |= (1<<1);
  80 }
  81
  82 void spi_servo_bypass(uint8_t enable)
  83 {
  84         if (enable)
  85                 spi_servo_set(BYPASS_ENABLE, 0);
  86         else
  87                 spi_servo_set(BYPASS_DISABLE, 0);
  88 }