/*
 * motionController.c
 * 
 * TMC429
 *
 *  Created on: 5 mei 2020
 *      Author: mreenen
 */

#include <msp430.h>
#include "typedefExtention.h"
#include "SPI.h"
#include "motionController.h"

const uchar MC_CS = BIT0;

// target position 24RW
#define MC_XTar1 0x00
#define MC_XTar2 0x20
#define MC_XTar3 0x40
// actual position 24R (RW if in velocity or hold mode)
#define MC_XAtu1 0x02
#define MC_XAtu2 0x22
#define MC_XAtu3 0x42
// velocity minimum (can be stopt emidetly) 11uRW
#define MC_VMin1 0x04
#define MC_VMin2 0x24
#define MC_VMin3 0x44
// velocity maximum 11uRW
#define MC_VMax1 0x06
#define MC_VMax2 0x26
#define MC_VMax3 0x46
// velocity target 12sRW
#define MC_VTar1 0x08
#define MC_VTar2 0x28
#define MC_VTar3 0x48
// velocity actual 12sR
#define MC_VAtu1 0x0A
#define MC_VAtu2 0x2A
#define MC_VAtu3 0x4A
// acceleration maximum 11uRW
#define MC_AMax1 0x0C
#define MC_AMax2 0x2C
#define MC_AMax3 0x4C
// acceleration actual 12sRW
#define MC_AAtu1 0x0E
#define MC_AAtu2 0x2E
#define MC_AAtu3 0x4E
// acceleration threshold 11uRW
#define MC_AAtu1 0x0E
#define MC_AAtu2 0x2E
#define MC_AAtu3 0x4E
// P value for ramp mode
#define MC_P1    0x12
#define MC_P2    0x32
#define MC_P3    0x52
// ramp_mode, ref conf and lp
#define MC_Rmcl1 0x14
#define MC_Rmcl2 0x34
#define MC_Rmcl3 0x54
#define MC_lp            0x010000
#define MC_REF_CONF      8
#define MC_RAMP_MODE     0b00
#define MC_SOFT_MODE     0b01
#define MC_VELOCITY_MODE 0b10
#define MC_HOLD_MODE     0b11
// deviders
#define MD_Div1  0x18
#define MD_Div2  0x38
#define MD_Div3  0x58
#define MD_PULSE_DIV 12
#define MD_RAMP_DIV  8
// IF_CONFIGURATION_429
#define MD_IF_CONF_429 0x68
#define MD_InvRef     0x001
#define MD_SdoInt     0x002
#define MD_StepHalf   0x004
#define MD_InvStp     0x008
#define MD_InvDir     0x010
#define MD_EnSd       0x020
#define MD_PosCompSel 6
#define MD_EnRefr     0x100


//TODO: make function to calculate and write P

void MC_write(uchar addr, ulong data) {
  char i;

  // wait for SPI bus to be available
  SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);

  // set write bit to write
  addr &= ~0x01;
  SPI_packet[1] = addr;

  // save data to SPI packet buffer
  for(i=4; i>1; i--){
    SPI_packet[i] = data;
    data >>= 8;
  }

  SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
  SPISend();
}

ulong MC_read(unsigned char addr) {
  char i;
  ulong data;

  // wait for SPI bus to be available
  SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);

  // set write bit to read
  addr |= 0x01;
  SPI_packet[1] = addr;

  SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
  SPISend();

  // wait for SPI bus to be available
  SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);

  // set write bit to read
  addr |= 0x01;
  SPI_packet[1] = addr;

  SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
  SPISend();

  // read data
  for(i=2; i<5; i++){
    data |= (ulong) SPI_packet[i];
    data <<= 8;
  }

  return data;
}

void MCInit(){
  ulong c;

  P1DIR |= MC_CS; // set MC_CS (pin 5) as output
  P1OUT |= MC_CS; // set MC_CS high


  // set min and max velocity
  MC_write(MC_VMin1, 0);
  MC_write(MC_VMin2, 0);
  MC_write(MC_VMax1, 2047);
  MC_write(MC_VMax2, 2047);

  // set clock dividers
  MC_write(MD_Div1, (9 << MD_PULSE_DIV) | (3 << MD_RAMP_DIV));
  MC_write(MD_Div2, (9 << MD_PULSE_DIV) | (3 << MD_RAMP_DIV));

  // max acceleration
  MC_write(MC_AMax1, 1);
  MC_write(MC_AMax2, 1);

  // set ramp mode and reference switch configuration
  MC_write(MC_Rmcl1, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);
  MC_write(MC_Rmcl2, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);

  // set to stap/dir mode
  c = MC_read(MD_IF_CONF_429);
  MC_write(MD_IF_CONF_429, c | MD_EnSd);

  //DEBUG
  MC_read(MC_VMax1);
  MC_read(MC_Rmcl2);
  MC_read(MC_AMax1);
}

void MCSetV(enum MC_motor m, int v){
  uchar addr = MC_VTar1 | m;

  // shorten the int
  if(v < 0)
    v |= 0x0800; // set signed bit
  else
    v &= ~0x0800; // remove signed bit
  v &= 0x0FFF; // contraint to 12 bits

  MC_write(addr, v);
}