True Torque controller

developement
8

Hey guys,
I’ve spent some time on this topic recently and using the new SimpleFOCShield v2.0 board I was able to god pretty good results.

Here is a video of the velocity control loop with current sensing + limiting:

The code that I used is the basically the same as the code from before. Here is the arduino

/**

   Torque control example using current control loop.

   This example is based on stm32 Nucleo-64 board in combintaiton with the Arduino SimpleFOCShield V2.0

   The current is measured inline with the shunt resistor or R= 0.01 Ohm, and amplification gain og G=50
*/
#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8);

// encoder instance
Encoder encoder = Encoder(2, 3, 500);

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

//MagneticSensorI2C encoder = MagneticSensorI2C(AS5600_I2C);

// center remove for bidirectional current sensing
float center_ia=0;
float center_ib=0;
void setup() {

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

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

  // set motion control loop to be used ( doing nothing )
  motor.controller = ControlType::voltage;

  // initialize motor
  motor.init();
  // align sensor and start FOC
  motor.initFOC();

  // use monitoring with serial
  Serial.begin(250000);
  
  _delay(1000);
  //  Serial.println("Align currents.");
  for (int i = 0; i < 500; i++) {
    center_ia += analogRead(A0);
    center_ib += analogRead(A2);
  }
  center_ia = center_ia / 500.0;
  center_ib = center_ib / 500.0;
  _delay(1000);
}

// target velocity
float target_velocity= 0;

// currents
float ia, ib,ic;
float ialpha, ibeta;
float id, iq;

// PID controllers and low pass filters
PIDController PIDv{0.2, 30, 0.005, 10000, 3};
LowPassFilter LPFv{0.001};
PIDController PIDq{5,1000,0,100000,12};
PIDController PIDd{5,1000,0,100000,12};
LowPassFilter LPFq{0.001};
LowPassFilter LPFd{0.001};


// counting variable for user display
long t = 0;
float target_iq = 0;

void loop() {
  // nucleo
   ia = (analogRead(A0) - center_ia)*0.00644;//*3.3/1024.0/50.0/0.01; // amps
   ib = -(analogRead(A2) - center_ib)*0.00644;//*3.3/1024.0/50/0.01; // amps

  // clarke
  ialpha = ia;
  ibeta = _1_SQRT3 * ia + _2_SQRT3 * ib;

  float theta = _normalizeAngle(_electricalAngle(motor.shaft_angle, motor.pole_pairs));
  float ct = _cos(theta);
  float st = _sin(theta);
  // park
  id = LPFd(ialpha * ct + ibeta * st);
  iq = LPFq(ibeta * ct - ialpha * st);

  motor.voltage_q = PIDq(target_iq - iq);   
  motor.voltage_d = PIDd(-id);


  // set the phase voltages Ud and Uq
  motor.loopFOC();


  // show user the currents
  if (!t) {
    target_iq = PIDv(target_velocity- LPFv(encoder.getVelocity())) / 10.0;
    Serial.print(iq * 1000); // mAmps
    Serial.print("\t");
    Serial.println(id * 1000); // mAmps
  }
  t > 10 ? t = 0 : t++;

  // communicate with the user
  tuneController();
}


// Serial communication callback function
// gets the target value from the user
void tuneController() {
  // a string to hold incoming data
  static String inputString; 
  while (Serial.available()) {
     // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline
    // end of input
    if (inChar == '\n') {
      if(inputString.charAt(0) == 'A'){
        PIDq.P = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'B'){
        PIDq.I= inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'C'){
        PIDd.P= inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'D'){
        PIDd.I = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'E'){
        LPFq.Tf=inputString.substring(1).toFloat(); 
      }else if(inputString.charAt(0) == 'F'){
        LPFd.Tf =inputString.substring(1).toFloat(); 
      }else if(inputString.charAt(0) == 'G'){
        PIDv.P = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'H'){
        PIDv.I = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'I'){
        PIDv.D = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'J'){
        LPFv.Tf = inputString.substring(1).toFloat();
      }else if(inputString.charAt(0) == 'k'){
        PIDv.limit= inputString.substring(1).toFloat()/100.0;
      }else{
        target_velocity=inputString.toFloat();
      }
      inputString = "";
    }
  }
}
1 Like