I am trying to control GM3506 motor with SimpleFOC v2.0.4 on arduino mega.
In TorqueControlType::dc_current , I am calibrating the PI for q axis but the process is not smooth. The motor keeps shaking around the target position. But when I change to TorqueControlType::voltage with PID for velocity and angle, everything is back to normal.
I am not sure which step I am doing wrong or the process I am calibrating the PID is not correct. Hope to receive answers from experts
Here is my code and the pid parameters I tried
[P=0,5]
[P=0,9]
[P=0,5 ; I=20]
[P=0,5 ; I=50]
Code:
#include <SimpleFOC.h>
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 53);
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, 54, 56);
float target_angle = 0;
bool is_button_Press=false;
void calibrations()
{
if(Serial.available() > 0)
{
char inCommand=Serial.read();
if(inCommand=='t')
{
target_angle=Serial.parseFloat();
}
if(inCommand=='p')
{
motor.PID_current_q.P=Serial.parseFloat();
}
if(inCommand=='i')
{
motor.PID_current_q.I=Serial.parseFloat();
}
if(inCommand=='f')
{
motor.LPF_current_q.Tf=Serial.parseFloat();
}
if(inCommand=='c')
{
motor.disable();
}
}
}
void setup() {
Serial.begin(115200);
pinMode(22, INPUT_PULLUP);
sensor.init();
motor.linkSensor(&sensor);
current_sense.gain_b *= -1;
current_sense.init();
current_sense.linkDriver(&driver);
motor.linkCurrentSense(¤t_sense);
driver.voltage_power_supply = 12;
driver.init();
motor.linkDriver(&driver);
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// motor.torque_controller = TorqueControlType::foc_current;
motor.torque_controller = TorqueControlType::dc_current;
motor.controller = MotionControlType::angle;
motor.PID_velocity.P = 0.07;
motor.PID_velocity.I = 5;
motor.PID_velocity.D = 0.001;
motor.LPF_velocity.Tf = 0.01;
motor.P_angle.P = 6;
motor.LPF_angle.Tf = 0.01;
// motor.PID_current_d.P = 0;
// motor.PID_current_d.I = 0;
// motor.PID_current_d.D = 0;
// motor.PID_current_d.limit = 1;
// motor.LPF_current_d.Tf = 0.01;
motor.PID_current_q.P = 0;
motor.PID_current_q.I = 0;
motor.PID_current_q.D = 0;
motor.PID_current_q.limit = 1;
motor.LPF_current_q.Tf = 0.005;
motor.voltage_limit = 2;
motor.velocity_limit = 5;
motor.sensor_direction=Direction::CW;
motor.sensor_offset=0.2;
motor.zero_electric_angle=3.4798;
motor.init();
motor.initFOC();
_delay(1000);
}
void loop()
{
if(digitalRead(22)==0 && !is_button_Press)
{
motor.disable();
Serial.println("stop");
}
calibrations();
motor.loopFOC();
motor.move(target_angle);
// plot
Serial.print(">Current sense:");Serial.println(current_sense.getDCCurrent());
Serial.print(">Angle_error:");Serial.println(target_angle-(target_angle-sensor.getAngle()));
Serial.print(">P:");Serial.println(motor.PID_current_q.P);
Serial.print(">I:");Serial.println(motor.PID_current_q.I);
//
}