Speed limit on B-G431B-ESC1 with 2212 920kv motor

Tried the simple angle limiting plan and it did not work with flux observer. Compared to unmodified open loop it reaches a bit higher speed before stalling, but I still have to go back to 0 target and ramp up to get moving again. Maybe it would work with a real position sensor, but I think we should give up on it and return to the plan of switching to closed-loop velocity for recovery. Revert the thread back to post #30 :slight_smile:

So the only changes to BLDCMotor should be:
loopFOC: Don’t bail out in velocity_openloop mode
velocityOpenloop: Don’t call setPhaseVoltage, do set electrical_angle

Sensor should not be linked to the motor in open loop mode, and motor.torque_controller should be TorqueControlType::dc_current.

Maybe should switch to TorqueControlType::foc_current in closed loop mode?

I thought the phase resistance shouldn’t be affecting open loop mode anymore. With setPhaseVoltage commented out of velocityOpenloop, the TorqueControlType::dc_current code in loopFOC should overwrite the voltage that was calculated there.

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(&currentSense);

  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();
}

Hope you are well. I am just coming back around to the phase resistance limit issue. I seem to be hitting a speed wall.

Snowed in with about 10 inches, but well stocked and no power outage :slight_smile:

Can you confirm if the dc_current code in loopFOC is actually running? I still don’t see how the phase resistance could affect open loop if the Uq calculated in velocityOpenloop is overwritten by the dc_current code before the call to setPhaseVoltage.

What is your maximum closed loop speed with the flux observer and TorqueControlType::foc_current, and no serial prints disturbing it? My 400kv Racerstar running on B-G431B-ESC1 with 12 volt power supply reaches 467rad/s (4460rpm), which is 93% of voltage x kv so I expect it could go faster with more volts. It has 11 pole pairs, so 4460x11=49000erpm, which would be 7000rpm on your 7pp motor. So you should be able to reach minimum 733rad/s and probably higher. Your 2450kv with 12 volts should be CPU limited rather than voltage limited, since voltage x kv works out to 3079rad/s. My loop time is around 60-65 microseconds, so about 16KHz. With the minimum 6 updates per electrical revolution, that’s 16000/6=2666erps=160000erpm=22857rpm=2393rad/s where it should definitely be CPU limited, though I’d be surprised if it can actually make it that high. But the 120rad/s you were getting with the i2c sensor back in the beginning is way lower than I’d expect flux observer to reach.

Congrats on the power! Hope the snow is pretty at least. Before diving in, I want to thank you dearly for your generosity. I hope I can help someone else to the extent that you have helped me.

I was just about to head to bed here in Europe, but ran some tests to double check. I should mention here that I am totally new to motor control and I might be ambitious with the loads that I am putting on these little motors. I can confirm that TorqueControlType::dc_current code is running btw. I have the higher 2300 kv motor running now and still need to test and tun with the 920 kv motor. So… these stats are for the 2300kv.

With no load, I am getting up to 500+ rad/s both in closed and open loop.

In closed loop, something is happening that past about 500 rad/s it stops reacting but keeps running at some constant speed. Perhaps still a result of the flux observer tuning. In open loop, it seems only limited by the voltage_limit. But it stopped at about 450 rads when motor.voltage_limit = 1

With load (a 3d printed impeller about 10 cm in diameter), no matter open or closed 200 rad/s seems to be a limit. In closed loop, it just does not go any faster. In open loop, it stops. I think it is a torque issue. There just isnt enough. Would that not be more related to both a current_limit and a voltage_limit and not just the latter?

That is why I bought the 920 kv motor thinking that it would be better suited for the task, but since trying out simpleFOC, I seem to be having better results with the higher kv motor. But now I am starting to feel more comfortable and tomorrow I will try to update you with stats for the 920 kv motor. With and without loads.

Just an update. I was able to tune the 920 kv motor. In open loop, I am getting good performance. Even on load. In closed loop, I am maxing out at around 175 rad/s with no monitoring. So I guess this is a sensor speed issue?

How can I test the loop time? Because your sensor seems to be running much faster.

Just call _micros() at the start and end of the loop and print the difference.

Is that still with motor.voltage_limit=1? 920kv x 2V=1840rpm=193rad/s, so 175 is 91% of theoretical max, which seems reasonable. Similarly with the 2300kv motor, 450rad/s with voltage limit 1 is 93% of theoretical max.

The reason it’s x 2V is because motor voltage is multiplied by sin/cos which go positive and negative, so actual voltage difference between phases can be double that. i.e. motor limit 1 is equivalent to driver limit 2. I find it terribly confusing, but that’s how the library is written. https://docs.simplefoc.com/foc_theory

Loading the motor will indeed start eating into your top speed since more of the voltage goes toward producing current. If the hardware current sense is running properly, it should be safe to use the full 7V limit (0.58 x power supply voltage, see that foc_theory page), although it certainly wouldn’t hurt to creep up on it and keep an eye on the temperature incase there’s some configuration problem allowing higher current than expected.

Current limit does affect top speed as well, especially with a propeller-type load where torque increases with rpm. For closed loop, the current limit should generally be set as high as you can without overheating the motor or driver when stalled. In open loop, you may or may not want it a bit lower to reduce energy waste and improve smoothness. And don’t forget you also need to set current_sp in open loop for the dc_current code to use (maybe should put this in velocityOpenloop so it happens automatically).

Speaking of things that should be automatic but aren’t, motor.voltage_limit is copied into PID_velocity.limit, PID_current_q.limit and PID_current_d.limit in motor.init, so any time you change motor.voltage_limit after that, you should update those as well. Personally I think the best library solution for that would be to change PIDController.limit to a pointer so it can directly access the motor variable.

So… I just tested again (no load; closed loop).

1v limit is approx 130 rad/s
2v limit is approx 160 rad/s
3v limit is approx 160 rad/s
4v limit is approx 170 rad/s

In closed loop, I do not have any of those set. Just:
motor.PID_velocity.limit = motor.voltage_limit;

I am not sure I follow exactly what else I need to do. Full closed loop code below:

/** 
 * B-G431B-ESC1 position motion control example with encoder
 *
 */
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h>

// Motor instance
//BLDCMotor motor = BLDCMotor(7, 0.14, 2450, 0.00003);
BLDCMotor motor = BLDCMotor(7, 0.15, 940, 0.000125);
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 target = 0;
float targetP = 0;
float targetI = 0;
// instantiate the commander
Commander command = Commander(Serial);
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(&currentSense);

  motor.sensor_direction= Direction::CW;
  motor.zero_electric_angle = 0;

  // set motion control loop to be used
  motor.controller = MotionControlType::velocity;
  //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 = 8;
  motor.voltage_limit = 4;
  //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('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() {
  // main FOC algorithm function
  motor.loopFOC();

  //motor.PID_velocity.P = targetP;
  //motor.PID_velocity.I = targetI;

  //Serial.println(target);

  // Motion control function
  motor.move(targetP);

  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  motor.monitor();
  
  // user communication
  command.run();
}

Use motor.torque_controller = TorqueControlType::foc_current; for closed loop. It’s using voltage-based current limiting right now, so I think the low resistance value necessary for flux observer to work is causing lower than expected applied voltage.

That worked!!! 420 rad/s at 3 v : ) And running quite efficiently. No load for now. There is some wonky behavior at startup and I had to retune the impedence. Still a big jerk at the beginning, but once I am moving it is ok. It is just the cross over from no movement to movement.

Ok. So. going back to the recovery idea. Would I also have to change between
motor.torque_controller = TorqueControlType::dc_current;
and
motor.torque_controller = TorqueControlType::foc_current;

I think it should work so long as I can stay in recovery mode until we are back to speed and not switch back to open loop until the recover is done even when angles align. E.g. this problem that I mentioned earlier.

Yeah, you’ll have to switch the torque controller for recovery mode.

420rad/s is a big improvement, but I would expect around 550rad/s for 3V (920kv x 6V=5520rpm=578rad/s theoretical max) so there may still be a problem somewhere. Try stepping the voltage again and see if we can spot a pattern in the max speeds.

I don’t notice much of a startup jerk on mine, but if you have it loose on the table then I would expect it to jump.

its loose on the table ; )

I stepped up to 4v and got up to 560 rad/s. Still 420 rad/s at 3v. Here is my motor setting.

BLDCMotor motor = BLDCMotor(7, 0.15, 940, 0.0000725);

I actually tried the recovery mode with open loop using
motor.torque_controller = TorqueControlType::foc_current;

Hope that was not a bad idea, but it worked!!! Still, I can tell the switch needs to be smarter.

Interesting. 560 is exactly 4/3 x 420, so it appears you have an effective 668kv.

I’m not familiar enough with all the transformations in the current sense code to say for sure whether foc_current will work for open loop, but logically it doesn’t make sense considering that the rotor angle is unknown. My objective was just to ensure that the total current remains within the limit so nothing will explode when stalled in open loop at high voltage. The objective of foc_current is to simultaneously enforce that limit and ensure that the direction of the current vector is 90 electrical degrees ahead of the rotor.

hmmm. I hope I am not doing anything wrong. I will double check tomorrow that the plumbing is correct. And to be safe will switch the torque control in recovery mode.

Should I leave my motor definition as is or should I keep tinkering?

I have one more question regarding the LPF in SimpleFoc. I think I will try it on the difference between the open loop and closed loop angle to make sure it does not sputter back and forth when there are coincidences in angle during the recover. Unless you have a better idea.

But I am also using it for the trajectory ramp up/down for the velocity in open loop and am noticing that the setting I have:

LowPassFilter targetFilter = LowPassFilter(2.0f);

Has a nice ramp on long runs but then takes a long, long time to reach the actual target. Is there a way to make the final arrival to the destination shorter?

Either way, I think we are close!

I’d leave the kv as is. I think there’s still some mystery to be solved.

The lowpass filter has an exponential decay, so if you want linear ramping you’ll have to do it yourself (look at the output ramp in PIDController).

For the recovery mode, I think you need separate conditions for switching in and out of closed loop. Angle out of range is the condition to switch from open to closed loop. Once you’re in closed loop, the angle will always be within range so it will immediately switch back. A “quick and easy” solution would be to run closed loop for a set amount of time and then switch back. A more proper condition would be to wait until the velocity is stable near the target.

Seems straight forward. I will work on this today and show you what I have later! Just have a few other things to attend to.

Again, thanks for the pointer. Just trying to keep my overhead down.

Hmmm. Would be nice to identify. Any ideas for troubleshooting welcome. Would further lowering the phase resistance help?

Just an update. I spent many hours trying to get the recovery mode to work. It seems so close, but the switch seems to mess up the flux observer.

But I am pretty pleased with the completely closed loop results and the pitch seems stable. I just need to test on load with the actual siren. I think you were right that my actual KV is lower. It really seems tuned nicely at about 850 kv.

I would like to come back to this and make it work, but now I need to move on to communication from an esp32 to the esc (which I will post about tomorrow since it seems pretty difficult to get Serial1 working from what I have read - but hopefully it is doable given that I am sensorless).

Nonetheless, I paste one of my attempts below for the timed switch (commented out). No go. The fast switch seems to actually work a bit better.

Again, thanks for all the help. At least I got the flux observer working. More soon.

// 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);
//BLDCMotor motor = BLDCMotor(7, 0.15, 940, 0.000125);
BLDCMotor motor = BLDCMotor(7, 0.14, 850, 0.0000525);

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(&currentSense);

  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.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 = 5;
  motor.voltage_limit = 3;
  //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();

  float targetF = targetFilter(targetV);
  motor.move(targetF);

  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 velocityInRangeTime;
  static uint32_t angleOutOfRangeTime;
  static uint32_t lastMillis;

  static bool recovery = false;

  /*static uint32_t lastPrintTime = millis();
	if (millis() > lastPrintTime + 500) {
	  lastPrintTime = millis();
	  Serial.print("\t\t\t\t\t");
	  Serial.print(sensorAngle);
	  Serial.print("\t");
		Serial.print(openloopAngle);
    Serial.print("\t");
		Serial.print(diff);
    Serial.print("\t");
    Serial.print(diffFilter(targetV - sensor.getVelocity()));
    Serial.print("\t");
    Serial.println(motor.controller);
	}
  */

/*
  if(abs(diff) > (32768/6)){
    angleOutOfRangeTime += (millis() - lastMillis);
  } else {
    angleOutOfRangeTime = 0;
  }

  if(diffFilter(abs(targetV - sensor.getVelocity())) < 10){
    velocityInRangeTime += (millis() - lastMillis);
  } else {
    velocityInRangeTime = 0;
  }

  if (angleOutOfRangeTime > 5) {
    motor.controller = MotionControlType::velocity;
    motor.torque_controller = TorqueControlType::foc_current;
  } 
  
  if(velocityInRangeTime > 500){
    motor.controller = MotionControlType::velocity_openloop;
    motor.torque_controller = TorqueControlType::dc_current;
  } 

  lastMillis = millis();
*/

  // This was the original alternative
  if (abs(diff) > (32768/6)){
    motor.controller = MotionControlType::velocity;
    motor.torque_controller = TorqueControlType::foc_current;
  } else {
    motor.controller = MotionControlType::velocity_openloop;
    motor.torque_controller = TorqueControlType::dc_current;
  }

  //motor.monitor();
  command.run();
}


Thanks for all your help!!!

I can’t get the video to play, but glad to hear closed loop is working well enough. That will certainly be simpler and more reliable anyway.

You’re the third person to try and fail at a pseudo-open loop solution, so I think we’re going to need the dogged determination of a PhD student in a well-equipped lab to ever make it work, or understand why it doesn’t :slight_smile:

I tried it for position control with hall sensors, to do precise positioning between steps after getting as close to the target as the sensors can measure. It always jumped when switching back to closed loop, which often resulted in vibration as it quickly switched back and forth. I didn’t devote much effort to it though, since my motors are geared down a lot so the coarse resolution was fine. And after trying @Nanoparticle’s linear hall technique, there’s no reason to use digital halls anymore aside from convenience if a motor comes with them pre-installed, or the driver (such as B-G431B-ESC1) doesn’t have two analog pins accessible.

@Anthony_Douglas was working on a ventilator fan that needed to be as silent as possible, and open loop gives the cleanest waves. But in his case it also needed to be high efficiency, which meant keeping the current just above stall. He may have ended up settling on flux observer too, I’m not sure.

Bummer about the video. Next week, I will try to get one online somewhere for you to see. I was unsure about the flux observer because in the switching code and putting a filter on it, it seemed to not be at the right speed. I thought that was part of the problem.

But I checked the “tuning” in closed loop mode and it was spot on! So, as you point out, for now I will keep things simple. There is still a bunch of jerkyness to get it started, so I might still put in a switch below 20 rads.

I think one problem might be the switch between the torque controllers. It might need to be smoother because that switch was making the flux observer go haywire. The switch back was not so great either. The idea about a morph between the two controllers might work because when the switch is based on angle with no time check, it did kind of work. It was just noise. So maybe the more involved solution is to morph from one torque controller to the other over some short amount of time.

Anyway, I am now pretty convinced my the flux observer. I checked against sine tones and it was almost perfect!