gramophonEngine

/**
 * Comprehensive BLDC motor control example using magnetic sensor
 *
 * Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
 * - configure PID controller constants
 * - change motion control loops
 * - monitor motor variabels
 * - set target values
 * - check all the configuration values
 *
 * See more info in docs.simplefoc.com/commander_interface
 */
#include <SimpleFOC.h>

// LOOP TIME
unsigned long time1, time2, loopt, period = 0;

const int speeed = 32;
float target_velocity = 0.0;

// POT
int potPin = A19; // pin 26
int potVdd = 27;
int potVal = 0;

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, 5);
// magnetic sensor instance - MagneticSensorI2C
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);


// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(16, 17, 21, 22);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

// inline current sensor instance
// ACS712-05B has the resolution of 0.185mV per Amp
InlineCurrentSense current_sense = InlineCurrentSense(185.0f, A4, A5);

// commander interface
Commander command = Commander(Serial);
void onMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {

  //POT SETUP
  pinMode(potVdd, OUTPUT); // voltage supply for potentiometer

  // use monitoring with serial
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
  SimpleFOCDebug::enable(&Serial);

  // initialise magnetic sensor hardware
  sensor.init();
  // link the motor to the sensor
  motor.linkSensor(&sensor);

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

  // choose FOC modulation
  //motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // set control loop type to be used
  motor.controller = MotionControlType::velocity;
  motor.torque_controller = TorqueControlType::foc_current; //foc_current;

  // contoller configuration based on the control type
  motor.PID_velocity.P = 0.8f;
  motor.PID_velocity.I = 1;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 24;

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

  // angle loop controller
  motor.P_angle.P = 20;
  // angle loop velocity limit
  motor.velocity_limit = 50;

  // comment out if not needed
  motor.useMonitoring(Serial);

  // current sense init and linking
  current_sense.init();
  motor.linkCurrentSense(&current_sense);

  // initialise motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // set the inital target value
  motor.target = -3.49; //-6.98;

  // define the motor id
  command.add('A', onMotor, "motor");

  // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
  Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));

  _delay(1000);
}


void loop() {
  // loop time measure
//  time1 = micros();

  // read the value from the pot:
  digitalWrite(potVdd, HIGH); // 3.3V to the pot
  potVal = analogRead(potPin);

  motor.target = -(3.49 + (potVal / 500.0));

  // iterative setting FOC phase voltage
  motor.loopFOC();

  //target_velocity = -3.49 + (analogRead(speeed)-2048)/1024.0;
  //Serial.println(target_velocity);

  // iterative function setting the outter loop target
  // velocity, position or voltage
  // if tatget not set in parameter uses motor.target variable
  //motor.move(target_velocity);
  motor.move();

  //PhaseCurrent_s currents = current_sense.getPhaseCurrents();
  //float current_magnitude = current_sense.getDCCurrent();

  //Serial.print(currents.a*1000); // milli Amps
  //Serial.print("\t");
  //Serial.print(currents.b*1000); // milli Amps
  //Serial.print("\t");
  //Serial.println(current_magnitude*1000); // milli Amps




  // user communication
  command.run();


//  time2 = micros();
//  loopt = time2 - time1;
//
  if(period >= 1000)
  {
//    Serial.print("loop time: ");
//    Serial.println(loopt);
    Serial.println(sensor.getAngle());
    period = 0;
  }
  else
    period++;


}