/*
Makerbase Gimbal Motor 2804 with an AS5600 magnetic encoder. The SimpleFOC library provides an easy way to control BLDC motors using Arduino.

Please note, you need to install the SimpleFOC library first. You can do that by going to Sketch > Include Library > Manage Libraries in the Arduino IDE, then search for SimpleFOC and install it.

The AS5600 communicates over the I2C protocol. Here is a sample code to run a gimbal motor using SimpleFOC:
In this example, a gimbal motor is linked to a magnetic sensor (AS5600). The voltage limit is set to 3 volts, and the motor will try to reach the target position provided via the Serial interface. You set a target position by sending a command like "T1.0" over Serial.

Remember to connect the sensor and driver pins correctly, and power the motor with an appropriate power source.

Disclaimer: The code might not work as expected if the encoder, motor, and driver are not connected and configured correctly. Always make sure to follow the guidelines provided in the datasheet/manual of your specific devices.
*/

#include <SimpleFOC.h>

// Magnetic sensor instance - AS5600
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

// BLDCMotor instance
BLDCMotor motor = BLDCMotor(11);

// BLDCDriver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);

void setup() { 

  // Initialise magnetic sensor hardware
  sensor.init();
  
  // Link the motor to the sensor
  motor.linkSensor(&sensor);

  // Choose FOC modulation (optional)
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // Initialize motor
  motor.init();
  
  // Link the motor to the driver
  motor.linkDriver(&driver);

  // Set voltage limit
  motor.voltage_limit = 3.0;

  // Use monitoring with the following function
  Serial.begin(115200);
  
  // Initialize motor
  motor.initFOC();
  
  // Add target command T
  SerialCommand cmdT = SerialCommand("T", [](Cmd* cmd)-> void {
    motor.target = cmd->getArg(0);
  });
  SerialManager.add(cmdT);
}

void loop() {
  // FOC algorithm function
  motor.loopFOC();
  
  // Monitoring function
  motor.monitor();
}