#include <SPI.h>    // include the SPI library:
#include "L6470.h"  // include the register and bit definitions

#define SLAVE_SELECT_PIN 10  // Wire this to the CSN pin
#define MOSI             11  // Wire this to the SDI pin
#define MISO             12  // Wire this to the SDO pin
#define SCK              13  // Wire this to the CK pin
#define dSPIN_RESET      7  // Wire this to the STBY line
#define dSPIN_BUSYN      6  // Wire this to the BSYN line

#define STAT1            4  // Hooked to an LED on the test jig
#define STAT2            5  // Hooked to an LED on the test jig
#define SWITCH           8   // Hooked to the switch input and a
                             //  pB on the jig

float testSpeed = 25;

void setup()
{
  // Standard serial port initialization for debugging.
  Serial.begin(9600);

  // pin configurations for the test jig
  pinMode(STAT1, OUTPUT);
  pinMode(STAT2, OUTPUT);
  pinMode(SWITCH, INPUT);
  pinMode(5, INPUT);
  digitalWrite(STAT1, LOW);
  digitalWrite(STAT2, LOW);


  dSPIN_init();


  if (dSPIN_GetParam(dSPIN_CONFIG) == 0x2E88)
    digitalWrite(STAT1, HIGH);


  dSPIN_SetParam(dSPIN_STEP_MODE,
                      !dSPIN_SYNC_EN |
                      dSPIN_STEP_SEL_1_16 |
                      dSPIN_SYNC_SEL_1);

  // Configure the MAX_SPEED register- this is the maximum number
  //  of (micro)steps per second allowed. You'll want to mess
  //  around with your desired application to see how far you can
  //  push it before the motor starts to slip. The ACTUAL
  //  parameter passed to this function is in steps/tick;
  //  MaxSpdCalc() will convert a number of steps/s into an
  //  appropriate value for this function. Note that for any move
  //  or goto type function where no speed is specified, this
  //  value will be used.
  dSPIN_SetParam(dSPIN_MAX_SPEED, MaxSpdCalc(400));

  dSPIN_SetParam(dSPIN_FS_SPD, FSCalc(50));


  dSPIN_SetParam(dSPIN_ACC, 0xfff);


  dSPIN_SetParam(dSPIN_OCD_TH, dSPIN_OCD_TH_6000mA);


  dSPIN_SetParam(dSPIN_CONFIG,
                   dSPIN_CONFIG_PWM_DIV_1 |
                   dSPIN_CONFIG_PWM_MUL_2 |
                   dSPIN_CONFIG_SR_180V_us |
                   dSPIN_CONFIG_OC_SD_DISABLE |
                   dSPIN_CONFIG_VS_COMP_DISABLE |
                   dSPIN_CONFIG_SW_HARD_STOP |
                   dSPIN_CONFIG_INT_16MHZ);


  dSPIN_SetParam(dSPIN_KVAL_RUN, 0xFF);

  // Calling GetStatus() clears the UVLO bit in the status
  //  register, which is set by default on power-up. The driver
  //  may not run without that bit cleared by this read operation.
  dSPIN_GetStatus();


  TCCR1A = 0;  // No waveform generation stuff.
  TCCR1B = B00000110; // Clock on falling edge, T1 pin.
  TCNT1 = 0;   // Clear the count.
}

void loop()
{
  dSPIN_Move(FWD, 25600);
  while (digitalRead(dSPIN_BUSYN) == LOW);  // Until the movement completes, the
                                            //  BUSYN pin will be low.
  dSPIN_SoftStop();                         // Inserting a soft stop between
                                            //  motions ensures that the driver
                                            //  will execute the next motion with
                                            //  the right speed.
  while (digitalRead(dSPIN_BUSYN) == LOW);  // Wait for the soft stop to complete.
  Serial.println(TCNT1, DEC);
  while(1);
}