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

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

const uchar MD_CS = BIT3;

// register 0x00 GCINF
#define MD_GCONF 0X00
#define MD_GCONF_singgelMotor   0x00000001
#define MD_GCONF_stepdir1Enable 0x00000002
#define MD_GCONF_stepdir2Enable 0x00000004
#define MD_GCONF_motor1Revers   0x00000010
#define MD_GCONF_motor1Revers   0x00000010
#define MD_GCONF_lockGCONF      0x00000020

// register 0x01 GSTAT
#define MD_GSTAT 0X01

// register 0x30, 0x50 IHOLD_IRUN
#define MD_IHIR1 0x30
#define MD_IHIR2 0x50
#define MD_IHIR_iHold      0
#define MD_IHIR_iRun       8
#define md_IHIR_iHoldDelay 16ul

// 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 MD_CC_vsens             0x00020000
#define MD_CC_tbl               15 // 3 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

// bool md_checkError(){
//   unsigned long stat = md_read(MD_GSTAT);
//   if(stat != 0){
//     if(stat == 0x1){ // only a recet has occert
//       // restart the motor driver
//       md_setup();
//     }else{ // one of the motorst stopt due to short or overheated or a undervoltage in chargepump
//       // wait a while for checking again
//       delay(1000);
//       md_checkError();
//     }
//     return true;
//   }
//   return false;
// }

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

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

  // 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(SPI_Mode3);
}


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

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

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

  SPISend(SPI_Mode3);

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

  return data;
}

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

  MD_write(MD_GCONF, MD_GCONF_stepdir2Enable | MD_GCONF_stepdir1Enable);
  MD_write(MD_CC1, 3ul<<MD_CC_toff | (4ul<<MD_CC_hstrt) | (1ul<<MD_CC_hend) | (2ul<<MD_CC_tbl) | (4ul<<MD_CC_mres) | MD_CC_16ustapI);
  MD_write(MD_CC2, 3ul<<MD_CC_toff | (4ul<<MD_CC_hstrt) | (1ul<<MD_CC_hend) | (2ul<<MD_CC_tbl) | (4ul<<MD_CC_mres) | 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);
}