#include <TMC429.h>


const long BAUD = 115200;
const int LOOP_DELAY = 1000;
const int CHIP_SELECT_PIN_429 = 10;
const int CLOCK_FREQUENCY_MHZ = 16;
const int MOTOR_COUNT = 3;
const int CHIP_SELECT_PIN_26X[MOTOR_COUNT] = {9,8,7};
const int CURRENT_SCALE_PERCENT = 18;
const int STEPS_PER_REV = 200;
const int MICROSTEPS_PER_STEP = 256;
const int REVS_PER_SEC_MAX = 2;
const int INC_PER_REV = 5;
const int VELOCITY_MIN = 100;
const int ACCELERATION_MAX = 50000;
const int MOTOR = 0;

TMC429 step_dir_controller;
long velocity_inc, velocity_max;
long target_velocity, actual_velocity;

void setup()
{
  // Setup serial communications
  Serial.begin(BAUD);

  step_dir_controller.setup(CHIP_SELECT_PIN_429,CLOCK_FREQUENCY_MHZ);

  bool communicating = step_dir_controller.communicating();
  Serial.print("communicating: ");
  Serial.println(communicating);

  const int microsteps_per_rev = STEPS_PER_REV*MICROSTEPS_PER_STEP;
  velocity_max = microsteps_per_rev*REVS_PER_SEC_MAX;
  velocity_inc = microsteps_per_rev/INC_PER_REV;
  for (int motor=0; motor<MOTOR_COUNT; ++motor)
  {
    step_dir_controller.setVelocityMode(motor);
    step_dir_controller.setLimitsInHz(motor,VELOCITY_MIN,velocity_max,ACCELERATION_MAX);
    step_dir_controller.disableLeftSwitchStop(motor);
    step_dir_controller.disableRightSwitchStop(motor);
  }

  target_velocity = -velocity_max;

}

void loop()
{
  target_velocity += velocity_inc;
  if (target_velocity > velocity_max)
  {
    step_dir_controller.stop(MOTOR);
    Serial.println("stopping motor!");
    delay(LOOP_DELAY*5);

    target_velocity = -velocity_max;
  }
  step_dir_controller.setTargetVelocityInHz(MOTOR,target_velocity);
  Serial.print("set target_velocity: ");
  Serial.println(target_velocity);
  Serial.print("at target_velocity: ");
  Serial.println(step_dir_controller.atTargetVelocity(MOTOR));

  target_velocity = step_dir_controller.getTargetVelocityInHz(MOTOR);

  do
  {
    actual_velocity = step_dir_controller.getActualVelocityInHz(MOTOR);
    Serial.print("actual_velocity: ");
    Serial.println(actual_velocity);
    delay(LOOP_DELAY);
  }
  while (actual_velocity != target_velocity);

  Serial.print("at target_velocity: ");
  Serial.println(step_dir_controller.atTargetVelocity(MOTOR));
  Serial.println();
}
