// Open loop motor control example
 #include <SimpleFOC.h> //2.3.1
#include "SimpleFOCDrivers.h" //1.0.5
#include "encoders/as5047/MagneticSensorAS5047.h"

#define WEB_INTERFACE

#define GL35

#ifdef GL35
BLDCMotor motor = BLDCMotor(7, 4.5, 100); 
#endif

BLDCDriver3PWM driver = BLDCDriver3PWM(PB4, PC7, PB10, PA9);

InlineCurrentSense currentsense = InlineCurrentSense(0.01f, 50.0f, A0, A2);

#define SENSOR1_CS PB6 // some digital pin that you're using as the nCS pin
MagneticSensorAS5047 sensor(SENSOR1_CS);

// instantiate the commander
Commander command = Commander(Serial);

void doMotor(char* cmd) { command.motor(&motor, cmd); }

void setup() {

  SPI.setMISO(PA6);
  SPI.setMOSI(PA7);
  SPI.setSCLK(PA5);
  
  Serial.println("sensor init");
  sensor.init(&SPI);

  // link sensor to motor
  Serial.println("motor link");
  motor.linkSensor(&sensor);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // limit the maximal dc voltage the driver can set
  // as a protection measure for the low-resistance motors
  // this value is fixed on startup
  
  #ifdef GL35
  driver.voltage_limit = 13;  //13/R = ~3A
  #endif

  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  currentsense.linkDriver(&driver);
  
  #ifdef GL35
  motor.voltage_limit = 4.5;   // [V] (1A)
  #endif

  motor.velocity_limit = 5;   
  motor.controller = MotionControlType::angle;
  motor.torque_controller = TorqueControlType::voltage;

  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01f;

  // angle loop controller
  motor.P_angle.P = 20;

  char motor_id = 'M';

  Serial.begin(115200);

  // contoller configuration based on the controll type
  motor.PID_velocity.P = 1;
  motor.PID_velocity.I = 0.05f;
  motor.PID_velocity.D = 0;
  
  // current sense init and linking
  currentsense.init();
  motor.linkCurrentSense(&currentsense);

  // init motor hardware
  motor.init();
  motor.initFOC();

  command.add(motor_id, doMotor,"motor");
 
  command.verbose = VerboseMode::machine_readable; // can be set using the webcontroller - optional  

  // command.
  Serial.begin(115200);
  _delay(1000);
}

void loop() {
  motor.loopFOC();
  
  motor.move();

  command.run();
}