This is almost working! I can get it to recover, but things are still not 100%. Code below. Here is why (I think):
I think the switch between modes needs some kind of filter otherwise even when in recovery mode, when the angles align it will temporarily switch back to openloop.
Either way, this speed limit is still a show stopper in recovery mode during which we are limited by the settings of the motor afaik.
And⌠as far as I gather empirically⌠under load I am reaching a limit in open loop. And even with no load, I reach a limit in closed loop.
I have not tested the recovery system on load, However, without load, I can see that past the closed loop velocity limit mentioned above, the recovery system gets wonky.
I think we are close. And some of these problems would get resolved with a sensor I assume.
Here is the code. Btw, with the following in loopFOC:
if (controller != MotionControlType::velocity_openloop)
electrical_angle = electricalAngle();
The sensor can remain linked to the motor so that it can be updated.
// Open loop motor control example
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h>
// Motor instance
BLDCMotor motor = BLDCMotor(7, 0.14, 2450, 0.00003);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
// encoder instance
MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor);
// velocity set point variable
float targetV = 0;
float targetP = 0;
float targetI = 0;
LowPassFilter targetFilter = LowPassFilter(2.0f);
LowPassFilter diffFilter = LowPassFilter(0.5f);
// instantiate the commander
Commander command = Commander(Serial);
void doTargetV(char* cmd) { command.scalar(&targetV, cmd); }
void doTargetP(char* cmd) { command.scalar(&targetP, cmd); }
void doTargetI(char* cmd) { command.scalar(&targetI, cmd); }
void setup() {
// use monitoring with serial
Serial.begin(115200);
// enable more verbose output for debugging
// comment out if not needed
SimpleFOCDebug::enable(&Serial);
// initialize encoder sensor hardware
sensor.init();
// link the motor to the sensor
motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
// link current sense and the driver
currentSense.linkDriver(&driver);
// initialise the current sensing
if(!currentSense.init()){
Serial.println("Current sense init failed.");
return;
}
// no need for aligning
currentSense.skip_align = true;
motor.linkCurrentSense(¤tSense);
motor.sensor_direction= Direction::CW;
motor.zero_electric_angle = 0;
// set motion control loop to be used
motor.controller = MotionControlType::velocity_openloop;
motor.torque_controller = TorqueControlType::dc_current;
//motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.current_sp = motor.current_limit;
// velocity PI controller parameters
motor.PID_velocity.P = 0.01;
motor.PID_velocity.I = 0.004;
driver.voltage_limit = 2;
motor.voltage_limit = 1;
//motor.current_limit = 2;
motor.PID_velocity.limit = motor.voltage_limit;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
//motor.PID_velocity.output_ramp = 1000;
// velocity low pass filtering time constant
//motor.LPF_velocity.Tf = 0.1;
// comment out if not needed
motor.useMonitoring(Serial);
motor.monitor_downsample = 10000;
//motor.motion_downsample = 4;
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// add target command T
command.add('V', doTargetV, "target V");
command.add('P', doTargetP, "target P");
command.add('I', doTargetI, "target I");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:"));
_delay(1000);
}
void loop() {
motor.loopFOC();
motor.move(targetFilter(targetV));
//Serial.println(motor.shaft_angle);
int32_t sensorAngle = (int32_t)(sensor.electrical_angle * (32768.0f/_PI));
int32_t openloopAngle = (int32_t)(motor.electrical_angle * (32768.0f/_PI));
int16_t diff = (int16_t)(sensorAngle - openloopAngle);
static uint32_t lastPrintTime = millis();
if (millis() > lastPrintTime + 250) {
lastPrintTime = millis();
Serial.print("\t");
Serial.print(sensorAngle);
Serial.print("\t");
Serial.print(openloopAngle);
Serial.print("\t");
Serial.print(diff);
Serial.print("\t");
Serial.println(motor.controller);
}
if ((diff > (32768/6) || diff < -(32768/6)) && motor.controller != MotionControlType::velocity) {
//Serial.println("here1");
motor.controller = MotionControlType::velocity;
} else {
//Serial.println("here2");
motor.controller = MotionControlType::velocity_openloop;
}
//motor.monitor();
command.run();
}
