Angle curve tuning - position control

[Papouc]

Hi guys,

sorry for my long disappearance (had an examination period at uni)… Nevertheless, I think the so called “motion profile” was the answer to my problem. I’ve implemented simple ramp profile as advised, which made raising the P gain possible.

I still need to have the LPF constant (for both velocity and angle) rather high (0.1 for both). But hey, it works :D. I think that the last issue might be hidden in the magnet choice. Even thought I am 100% sure that it is the correct type, it just might be of low quality or something. Maybe I’ll try to replace it in the future.

Here is a video of the result:

And my code (hope it helps someone with similar issue :D)

#include <SimpleFOC.h>

// --- ENCODER ---
MagneticSensorSPI sensor(AS5048_SPI, A5);


// --- DRIVER ---
#define INH_A 6
#define INH_B 5
#define INH_C 3
#define ENABLE A1

#define M_PWM 4
#define OC_ADJ A0

BLDCDriver3PWM driver(INH_A, INH_B, INH_C, ENABLE);

// --- BLDC MOTOR ---
#define POLE_PAIRS 20

BLDCMotor motor(POLE_PAIRS);

// motion profile
void generateProfile(float currentPos, float desiredPos, float timeToReach); // time in seconds
float readProfile(unsigned long timeFromStart);
unsigned long startTime;
float moveDurationTime;
float desiredTarget;

float k = 0.0f; // parameters of linear interpolation (y = kx + q)
float q = 0.0f;

void setup() 
{
  Serial.begin(115200);

  SimpleFOCDebug::enable();

  sensor.init();
  motor.linkSensor(&sensor);

  // deselect CAN interface SPI device
  pinMode(10, OUTPUT);
  digitalWrite(10, HIGH);

  // external driver setup
  pinMode(M_PWM, OUTPUT);
  digitalWrite(M_PWM, HIGH);

  pinMode(OC_ADJ, OUTPUT);
  digitalWrite(OC_ADJ, HIGH);

  // set driver voltage
  driver.voltage_power_supply = 20.0f;

  driver.init();
  motor.linkDriver(&driver);

  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  // set torque control loop type and controller
  motor.controller = MotionControlType::angle;
  motor.torque_controller = TorqueControlType::voltage;

  motor.voltage_sensor_align = 0.6f;
  motor.voltage_limit = 3.0f;

  // velocity PID tuning (+ velocity limits)
  motor.PID_velocity.P = 0.9f;
  motor.PID_velocity.I = 6.0f;
  motor.PID_velocity.D = 0.0f;

  motor.PID_velocity.output_ramp = 300;
  motor.LPF_velocity.Tf = 0.1f;

  motor.velocity_limit = 8.0f;

  // angle PID tuning
  motor.P_angle.P = 7.0f;
  motor.P_angle.I = 0.0f;
  motor.P_angle.D = 0.0f;

  motor.P_angle.output_ramp = 30;
  motor.LPF_angle.Tf = 0.1f;

  // foc properties obtained through torque mode
  motor.sensor_direction = Direction::CCW;
  motor.zero_electric_angle = 2.2f;

  motor.target = 0.0f;

  motor.init();
  motor.initFOC();

}

void loop() 
{
  if (Serial.available() > 0)
  {
    // PID tuning through serial line

    char component = Serial.read();
    float num = Serial.parseFloat();

    switch (component)
    {
      case 'P':
        motor.P_angle.P = num;
        break;
      case 'I':
        motor.P_angle.I = num;
        break;
      case 'D':
        motor.P_angle.D = num;
        break;
      case 'R':
        motor.P_angle.output_ramp = num;
        break;
      case 'F':
        motor.LPF_angle.Tf = num;
        break;
      case 'T':
        desiredTarget = num;
        moveDurationTime = abs(motor.shaft_angle - desiredTarget) * (1.1f/(2*PI));
        generateProfile(motor.shaft_angle, desiredTarget, moveDurationTime); // 2pi rad takes around 1.5 seconds
        startTime = _micros();
        break;
      default:
        break;
    }

    Serial.print("P: ");
    Serial.println(motor.P_angle.P, 3);

    Serial.print("I: ");
    Serial.println(motor.P_angle.I, 3);

    Serial.print("D: ");
    Serial.println(motor.P_angle.D, 3);

    Serial.print("R: ");
    Serial.println(motor.P_angle.output_ramp);

    Serial.print("Tf: ");
    Serial.println(motor.LPF_angle.Tf, 3);

    Serial.print("Target: ");
    Serial.println(desiredTarget);
    Serial.println();
  }

  if (_micros() - startTime >= (unsigned long)(moveDurationTime * 1000000)) // should be shaft angle probably
  {
    motor.target = desiredTarget;
  }
  else 
  {
    motor.target = readProfile(_micros() - startTime); 
    // Serial.println(motor.target);
  }

  motor.loopFOC();
  motor.move(); 
}

void generateProfile(float currentPos, float desiredPos, float timeToReach)
{
  // x1 = 0 (time starts at zero)
  // y1 = currentPos

  // x2 = timeToReach
  // y2 = desiredPos
  
  k = (currentPos - desiredPos) / -timeToReach;
  q = currentPos;

  Serial.println(k);
  Serial.println(q);
}

float readProfile(unsigned long timeFromStart)
{
  float t = timeFromStart / (float)1000000;
  return k * t + q;
}

So thank you @dekutree64 and @An_Hoang for your help!!

Adam