/*
 * motorDriver.c
 *
 *  Created on: 4 mei 2020
 *      Author: mreenen
 */

#include <msp430.h>
#include "SPI.h"
#include "motorDriver.h"

const uchar MD_CS = BIT3;

// register 0x00 GCINF
#define MD_GCONF 0x00
#define MD_GCONF_singgelMotor   BIT00
#define MD_GCONF_stepdir1Enable BIT01
#define MD_GCONF_stepdir2Enable BIT02
#define MD_GCONF_motor1Revers   BIT08
#define MD_GCONF_motor2Revers   BIT09
#define MD_GCONF_lockGCONF      BIT10

// register 0x01 GSTAT
#define MD_GSTAT 0x01
#define MD_GSTAT_recet    BIT00
#define MD_GSTAT_drv_err1 BIT01
#define MD_GSTAT_drv_err2 BIT02
#define MD_GSTAT_un_cp    BIT03

// register 0x30, 0x50 IHOLD_IRUN
#define MD_IHIR1 0x30
#define MD_IHIR2 0x50
#define MD_IHIR_iHold      0  // 5 bitts ( (x+1)/32 A )
#define MD_IHIR_iRun       8  // 5 bits
#define md_IHIR_iHoldDelay 16 // 4

// register 0x6C, 7C CHOPCONF
#define MD_CC1 0x6C
#define MD_CC2 0x7C
#define MD_CC_shortProtection   0x40000000
#define MD_CC_doubbleEdge       0x20000000
#define MD_CC_16ustapI          0x10000000
#define MD_CC_mres              24 // 4 bits; 1/(2^x / 2^8) microstappen
#define MC_CC_FULLSTEP          8ul << MD_CC_mres
#define MC_CC_2US               7ul << MD_CC_mres
#define MC_CC_4US               6ul << MD_CC_mres
#define MC_CC_8US               5ul << MD_CC_mres
#define MC_CC_16US              4ul << MD_CC_mres
#define MC_CC_32US              3ul << MD_CC_mres
#define MC_CC_64US              2ul << MD_CC_mres
#define MC_CC_128US             1ul << MD_CC_mres
#define MC_CC_256US             0ul << MD_CC_mres
#define MD_CC_vsens             BIT17
#define MD_CC_tbl               15 // 2 bits
#define MD_CC_cmh               BIT14
#define MD_CC_rndtf             0x00002000
#define MD_CC_disfdcc           0x00001000
#define MD_CC_fd3               BTI11
#define MD_CC_hend              7 // 4 bits
#define MD_CC_hstrt             4 // 3 bits
#define MD_CC_toff              0 // 4 bits

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

  // wait for SPI bus to be available
  SPIInitSend(MD_CS, SPI_Mode3, SPI_Clock_4MHz);

  // set write bit to write
  addr |= 0x80;
  SPI_packet[0] = addr;

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

  SPISend();
}


ulong MD_read(uchar addr) {
  char i;
  ulong data;

  // wait for SPI bus to be available
  SPIInitSend(MD_CS, SPI_Mode3, SPI_Clock_4MHz);

  // set write bit to read
  addr &= ~0x80;
  SPI_packet[0] = addr;

  SPISend();

  // wait for SPI bus to be available
  SPIInitSend(MD_CS, SPI_Mode3, SPI_Clock_4MHz);
  
  SPI_packet[0] = addr;
  SPISend();
  
  // read data
  for(i=2; i<5; i++){
    data |= (ulong) SPI_packet[i];
    data <<= 8;
  }

  return data;
}

enum bool MDStatus(){
  ulong stat = MD_read(MD_GSTAT);
  if(stat != 0){
    if(stat == 0x1){ // only a recet has occert
      // restart the motor driver
      MDInit();
    }else{ // one of the motors stopt due to short or overheated or a undervoltage in chargepump
      // wait a while for checking again
      __delay_cycles(16000000);
      MDStatus();
    }
    return true;
  }
  return false;
}

void MDInit(){
  P1DIR |= MD_CS; // set MD_CS (pin 3) as output
  P1OUT |= MD_CS; // set MD_CS high

  MDStatus();

  MD_write(MD_GCONF, MD_GCONF_stepdir2Enable | MD_GCONF_stepdir1Enable | MD_GCONF_motor2Revers | MD_GCONF_motor1Revers);
  MD_write(MD_CC1, 3ul<<MD_CC_toff | (4ul<<MD_CC_hstrt) | (1ul<<MD_CC_hend) | (2ul<<MD_CC_tbl) | MC_CC_16US | MD_CC_16ustapI);
  MD_write(MD_CC2, 3ul<<MD_CC_toff | (4ul<<MD_CC_hstrt) | (1ul<<MD_CC_hend) | (2ul<<MD_CC_tbl) | MC_CC_16US | MD_CC_16ustapI);
  MD_write(MD_IHIR1, 10ul<<MD_IHIR_iHold | 31ul<<MD_IHIR_iRun | 6ul<<md_IHIR_iHoldDelay);
  MD_write(MD_IHIR2, 10ul<<MD_IHIR_iHold | 31ul<<MD_IHIR_iRun | 6ul<<md_IHIR_iHoldDelay);

  MDStatus();
}