/**
 *
 * Torque control example using voltage control loop.
 *
 * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
 * you a way to control motor torque by setting the voltage to the motor instead of the current.
 *
 * This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
 */
 #define polepairs 11
 #define seconds_to_ramp

 #include <Arduino.h>
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/smoothing/SmoothingSensor.h>
BLDCMotor motor = BLDCMotor(polepairs);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);
// hall sensor instance
HallSensor sensor = HallSensor(PC6, PB11, PA3, polepairs);
SmoothingSensor smooth(sensor, motor);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}


// voltage set point variable
float target_voltage = 17;

void setup() {
  pinMode(PC6, INPUT_PULLUP);  // LED_BUILTIN
  pinMode(PB11, INPUT_PULLUP);
  pinMode(PA3, INPUT_PULLUP);
  smooth.phase_correction = -_PI_6;
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
 // SimpleFOCDebug::enable(&Serial);

  // initialize encoder sensor hardware
  sensor.init();
  sensor.enableInterrupts(doA, doB, doC);
  // link the motor to the sensor
  motor.linkSensor(&smooth);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  driver.init();
  // link driver
  motor.linkDriver(&driver);

  // aligning voltage
  motor.voltage_sensor_align = 5;

  // choose FOC modulation (optional)
  motor.foc_modulation = FOCModulationType::SinePWM;

  // set motion control loop to be used
  motor.controller = MotionControlType::torque;
  // initialize motor
  motor.init();
  // align sensor and start FOC
  motor.initFOC();
 for (float i = 0.0f; i < 600000.0f; i++){// assume 33 khz is what motor.loopfoc runs at if looped so 0.03 msec per loop so 600,000 should be 18 sec
  motor.loopFOC();
  motor.move(i*target_voltage/600000);
 }
}


void loop() {

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz
  motor.loopFOC();

  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code
  motor.move(target_voltage);
}