My hardware is STM32F407G-Discovery, L6234D, AS5048 SPI, motor - BGM2606-90T, inline current sense ACS70331. Open-loop velocity works well. I tested the sensor separately. The third digit changes, two decimal places are stable. In SimpleFOC Studio, the angle fluctuation with the drive disabled is 0.3 degrees. The problem occurs when switching to velocity mode with FOC current. First, I try to tune the Q and D current PIDs (yes, my PIDs are small, but with recommended values motor motor consumes abnormal current and jerks.) in torque mode with FOC current. My question here is: should the motor rotate, or should it stand still with torque/tension? The best I have achieved is motion with uneven movement, small (about a degree) movement at zero reference(in velocity-FOC mode). Below debug info and code
TIM3-CH1 TIM3-CH2 TIM3-CH3 score: 1
TIM3-CH1 TIM3-CH2 TIM8-CH3 score: 2
TIM3-CH1 TIM8-CH2 TIM3-CH3 score: 2
TIM3-CH1 TIM8-CH2 TIM8-CH3 score: 2
TIM8-CH1 TIM3-CH2 TIM3-CH3 score: 2
TIM8-CH1 TIM3-CH2 TIM8-CH3 score: 2
TIM8-CH1 TIM8-CH2 TIM3-CH3 score: 2
TIM8-CH1 TIM8-CH2 TIM8-CH3 score: 1
STM32-DRV: best: TIM3-CH1 TIM3-CH2 TIM3-CH3 score: 1
STM32-DRV: Initializing TIM3
STM32-DRV: Timer resolution set to: 1680
STM32-DRV: Configured TIM3_CH1
STM32-DRV: Configured TIM3_CH2
STM32-DRV: Configured TIM3_CH3
STM32-DRV: Synchronising 1 timers
0.94
MOT:Init
MOT:Enable driver.
=== MANUAL ALIGNMENT ===
This will rotate the motor to find zero angle.
Make sure the motor can rotate freely!
MOT:Align sensor.
MOT:sensor dir: CCW
MOT:PP check: OK!
Skip dir calib.
MOT:Zero elec. angle: 3.91
MOT:Align current sense.
CS: Switch B-(C)NC
CS: Inv C
MOT:Success: 4
MOT:Ready.
MOT:Monitor enabled!
> Inbox:
#include <SimpleFOC.h>
MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB2);
//BLDCMotor motor = BLDCMotor(6,12.3,108);
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(PC6, PC7, PC8, PB3, PB5, PB7);
InlineCurrentSense current_sense = InlineCurrentSense(200.0f, PA0, PA1, _NC );
Commander command = Commander(Serial);
float target_value = 0;
void doTarget(char* cmd) { command.scalar(&target_value, cmd); }
void doMotor(char* cmd) { command.motor(&motor, cmd);}
void setup() {
Serial.begin(9600);
SimpleFOCDebug::enable(&Serial);
// 1. sensor
delay(1000);
sensor.init();
//sensor.min_elapsed_time = 0.001;
motor.linkSensor(&sensor);
// 2. DRW
driver.voltage_power_supply = 12;
driver.voltage_limit = 12;
driver.init();
motor.linkDriver(&driver);
Serial.println(sensor.getAngle());
// 3. CS
current_sense.linkDriver(&driver);
current_sense.init();
//current_sense.gain_b *= -1;
//current_sense.gain_a *= -1;
motor.linkCurrentSense(¤t_sense);
// 4. calib
motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::velocity;
motor.voltage_limit = 12;
motor.current_limit = 2.0;
// 5. init
motor.init();
// 6.
Serial.println("=== MANUAL ALIGNMENT ===");
Serial.println("This will rotate the motor to find zero angle.");
Serial.println("Make sure the motor can rotate freely!");
delay(2000);
//
motor.voltage_sensor_align = 2.0; //
motor.initFOC(); //
// // 7.
// Serial.print("Sensor direction: ");
// Serial.println(motor.sensor_direction == Direction::CW ? "CW" : "CCW");
// Serial.print("Zero electric angle: ");
// Serial.println(motor.zero_electric_angle, 4);
motor.useMonitoring(Serial); //-------------------------------------------------------------------------------
// // 8.
// Serial.println("=== SAVE THESE VALUES ===");
// Serial.print("motor.sensor_direction = Direction::");
// Serial.println(motor.sensor_direction == Direction::CW ? "CW;" : "CCW;");
// Serial.print("motor.zero_electric_angle = ");
// Serial.print(motor.zero_electric_angle, 4);
// Serial.println(";");
// // 9.
// motor.torque_controller = TorqueControlType::foc_current;
// motor.controller = MotionControlType::velocity;
// velocity loop PID
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 2.0;
motor.PID_velocity.D = 0.0;
motor.PID_velocity.output_ramp = 1000.0;
motor.PID_velocity.limit = 12.0;
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0.007;
// angle loop PID
motor.P_angle.P = 0.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 0.0;
motor.P_angle.limit = 20.0;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.0;
// current q loop PID
motor.PID_current_q.P = 1.0;
motor.PID_current_q.I = 50.0;
motor.PID_current_q.D = 0.0;
motor.PID_current_q.output_ramp = 0.0;
motor.PID_current_q.limit = 12.0;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 0.01;
// current d loop PID
motor.PID_current_d.P = 1.0;
motor.PID_current_d.I = 50.0;
motor.PID_current_d.D = 0.0;
motor.PID_current_d.output_ramp = 0.0;
motor.PID_current_d.limit = 0.0;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 0.01;
// Limits
motor.velocity_limit = 20.0;
motor.voltage_limit = 12.0;
motor.current_limit = 2.0;
// sensor zero offset - home position
motor.sensor_offset = 0.0;
// general settings
// motor phase resistance
//motor.phase_resistance = 3.957;
// pwm modulation settings
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.modulation_centered = 1.0;
//command.add('T', doTarget, "target current");
command.add('M',doMotor,"motor");
//sensor.min_elapsed_time = 0.01; // 1ms - default value is 0.0001 (100us)
// Serial.println("Motor ready in TORQUE mode");
// Serial.println("Commands: T0.1 = 100mA");
}
void loop() {
motor.loopFOC();
motor.move();
motor.monitor();
command.run();
// static unsigned long lastPrint = 0;
// if (millis() - lastPrint > 1000) {
// lastPrint = millis();
// Serial.print("Iq: ");
// Serial.print(motor.current.q * 1000, 0);
// Serial.print(" mA | Vel: ");
// Serial.print(motor.shaft_velocity, 2);
// Serial.print(" | Angle: ");
// Serial.println(motor.electrical_angle, 3);
// }
}