# Concept for a super simple yet reasonably good sensorless driver with SimpleFOC

Ok, I am thinking about this a lot, because over the last year or so I learned a lot about motor drives, and I think I have come across a really simple approach to sensorless drive that would be pretty broadly applicable and relatively easy to implement. I have tried no less than 9 different sensorless motor drivers and they are all bunk for one reason or another, only one of them could actually even drive the motor that I’m using right now, which is not an unusual motor. Secondly, even that one was too loud, and not reversible, and not constant velocity and not something that can be reliably sourced etc. etc.

Basically, the concept is this:

In sensorless FOC systems, there are many different approaches but basically they all try to acertain the rotor position by watching the current waveform and voltage waveform and using motor characteristics, usually kV rating or volts per hz, resistance and inductance. That they are complicated is no real excuse imo for not implementing them as only one person has to do it once and then anyone can use the code. The math is already figured out, if you can understand it which I am not really there any more unfortunately having forgotten most of that stuff…

However, in reality we then use the rotor position and compare it with the magnetic field position (which is not quite the same as the electrical angle/the phase of the voltage waveform, but pretty close).

Then we regulate the field position so that it is optimal wrt the rotor position. A sensor of some kind is the obvious approach but they are expensive compared to cheap electronics and code.

Ok, so the critical realization is that simply watching the current waveform’s amplitude basically does most of the computation involved in all that stuff etc. for us. If you want the pieces, that’s hard to get. But getting the summary is actually easy, and that’s all we want anyway.

It factors in acceleration, inductance, resistance, saliency, everything all at once. All we need to really do is optimize the RPM to achieve minimal peak current amplitude for a given drive voltage, I think, or at least minimal average power supply current. I have experimented with my fan to make sure this appears to be correct, and indeed maximal efficiency appears to occur at the current minima, as indicated by the currensense.getDCcurrent() function, whatever exactly it is doing, it appears to be basically the same value as the peak current of any given phase, but more responsive.

The complicating factor is that instead of using a pi controller to achieve a setpoint along a function of positive or negative slope only, we now have a u shaped curve with negative slope in one area and positive in another.

So we can’t just use a PI controller. However there are many reasonably fast optimization methods for finding local minima of a function by adjusting the rads per second (RPS). I was thinking a basic hill climbing thing could be reasonably easy to implement. Basically a P controller, a pid controller with zero I and zero D gain. Then have a separate function which detects the polarity of the P controller, and applies it, by watching for changes in the RPS and relating them to changes in current. Then a function that adjusts the setpoint, to be X lower than the lowest value seen, but then also makes it creep up slowly. The up-creep rate would fight the P gain of the controller, which is not ideal, but should still work reasonably well I think.

You still have to tune the P gain which is a drag but probably not too hard. The system moves pretty fast so some kind of function that gave an indication of if things are oscillating and if so by how much might be a good idea. That’s one step down the road to some kind of auto-tune.

The optimizer speed would limit the acceleration rate and any other changes in loading, which might be an issue.

Doesn’t work at low RPM of course, but looking at the data with my 280kV motor I think it would be doable down to 300 rpm or so.

This system could also be implemented in such a way that you adjusted the voltage to optimize the current, but I think that would not be as responsive as it would be laggy due to the inertia of the rotor.

The optimizer has to be reasonably fast, though. That’s the main challenge, I think. If it’s just a fan it only has to deal with acceleration or sudden gusts of wind or whatever, otherwise it could be slow as molasses.

Just thought I would post here now instead of saying nothing, I’m not sure if I will even get the chance to try this, unfortunately as I already have my sensor based system nearly done and I’m out of budget :(. I wish I knew about this possibility earlier. It did occur to me, but at the time I thought it was kind of a crackpot approach that had a good chance of not working, but now I realize it is actually reasonably promising as you are doing something pretty similar to the other systems, actually. Again, it all depends on how fast the optimizer can work.

If I did try to do this it could probably only be a quick experiment, because I think it would only save me \$15 per unit or something so I can’t really spend another \$1000 developing it :(.

The cool thing is that I think it would work for any motor basically, and probably any waveform (trap, space vector etc) with only tuning the gain etc. and otherwise minimal changes. Those other motor drivers I tried, I’m sure those guys knew a lot about what they were doing but they still weren’t able to cook up something that had reasonably broad applicability. Sure didn’t work for me, I can say that much.

P.s. It would be great if there was an optimizer made for arduino that was ready to use…

I just want to drop a note: I have found that with several different motors, using several different drivers, that the efficiency of my fan can be seriously improved by using current optimization, in contrast to the other approaches commonly used, and I think I can see why, too.

Using several other motors but with the exact same fan blades and RPM, I have found that the motor current draw is about 260 mA with drivers like the ones I got on aliexpress but also the texas instruments [

### MCF8316A

](MCF8316A data sheet, product information and support | TI.com) driver, which I experimented with using the dev board, which I bought, it is the same. That’s a pretty high quality driver made by pretty talented people.

However, by manually doing current optimization by adjusting the voltage at the test RPS (250 rads per second), I can get the current down to 180 mA. The power consumption is thus 180/260 for this approach. That’s a massive, massive improvement in efficiency and as a result a massive improvement in potential power output for a motor that’s definitely worth noting and pursuing.

Basically, it’s the risk of stall combined with measurement error, I think. Other people call the relationship between the (measured) rotor position and the (assumed) magnetic field position the motor “timing”. I refer to it as the phase or angle diff usually, but whatever.

However, the key thing is that to get the optimal drive you need to get within like 0.05 rads or something. Error due to signal/code propagation, inaccuracy of the sensor and so on, and assumptions made (for instance, the assumption that the electrical angle is the same as the magnetic angle, which is not right as the magnetic angle gets delayed for several reasons, including inductance and salience), will lead to stall i if there isn’t a fudge margin to eat up (which implies suboptimal efficiency) or excessive current consumption, or both, depending on the strategy chosen.

The net result is that current optimization, if it can be made fast enough, also can get probably the best power output and performance. However the big thing which is probably why it’s not used, is probably the risk of stall, I think. The system has to be fast enough to keep a lock between rotor rpm and drive frequency, once it looses the lock it won’t be able to find it again I think , unless you used a considerably more advanced and fast technique to analyze the waveform, which is certainly possible.

All a question of how fast the system can be - I think probably you can detect the current and change the frequency way faster than the rotor can change speed so it shouldn’t be a big problem?

@Anthony_Douglas

Is the work performed the same? I mean at 180 do you move the same volume of air as 280mA?

Cheers,
Valentine

Yeah, it’s exactly the same, it’s a fan in free space, no impediments on either side. Pretty wack. Same blades, same rpm, gotta be pretty close to the same power output, right?

1 Like

If you get the same RPM in the same enclosure then that’s really impressive. Very educational. I’ve never compared with a commercial driver.

Cheers,
Valentine

1 Like

Hm I did a quick experiment today to see if I could make this happen. It does work but it’s extremely slow, and it tends to overshoot and cause stall if I increase the P gain any more. The problem seems to be that the polarity detection keeps flipping direction. It needs a longer sampling period I think, the noise is too large right now.

``````#include <SimpleFOC.h>

// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.01);

bool voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float p_gain = 0.1;// rads per second per amp
int controller_polarity = 1;
float stpt_adjust_rate = 0;//in amps per second
float setpoint_down_bias = 0.05; // in amps
float lowest_seen_current =1;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;

case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;

case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(p_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'Q':  break;
case 'S':  break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.

}
}
if (Serial.available() >= 9){
{

case 'T': goal_v = Serial.parseFloat();break;
case 'C': p_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':p_gain = Serial.parseFloat();break;
case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;

}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
//  float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
//  if (get_mA() > current_cap){
//    voltage_override = 0;
//  }
//}

static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
}
unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return  1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float p_controller(float clf){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float error = clf-setpoint;
float p_term = p_gain*error*time_since_last_call_s*controller_polarity*(-1);
float new_rps = motor.target+p_term;

Serial.print("mA:");
Serial.print(get_mA());
Serial.print(",");
Serial.print("rps:");
Serial.print(new_rps/100);
Serial.print(",");
Serial.print("p:");
Serial.print(p_term*100);
Serial.print(",");
Serial.print("error:");
Serial.println(error);

return new_rps;
}
int detect_polarity(float clf, float rps){
static float last_rps = rps;
float rps_diff = rps-last_rps;
last_rps = rps;
static float last_clf=clf;

float clf_diff = clf-last_clf;
last_clf=clf;
if (rps_diff >= 0){
if (clf_diff >= 0){
return 1;
}
if (clf_diff < 0){
return -1;
}
}
if (rps_diff < 0){
if (clf_diff > 0){
return -1;
}
if (clf_diff <= 0){
return 1;
}
}
else{
return 1;//that shouldn't happen but just in case
}
}
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}

if (clf<lowest_seen_current){
lowest_seen_current = clf;
}

return (lowest_seen_current-setpoint_down_bias);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
driver.init();

// link the motor and the driver
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2;   // [V]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
Serial.println(setpoint);
Serial.println(get_mA());
}

}

//float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
//   float x =0;
//   x = currentlf_now*motor.voltage_limit/24;
//   return  1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
//}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
//     unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;

for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
controller_polarity = detect_polarity(get_mA(), motor.target);

motor.target = p_controller(get_mA());

//     overcurrent_trip();
SerialComm();
}
``````

This one stabilizes within a few seconds and optimizes properly but again quite slow, the next step would be to make all the variables that need to be tuned accessible over serial and see how responsive I could make it. It stalls if you reduce the voltage too fast, of course.

``````
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.1);

bool voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float p_gain = 0.3;// rads per second per amp
int controller_polarity = 1;
float stpt_adjust_rate = 30;//in milliamps per second
float setpoint_down_bias = 2; // in milliiamps
float lowest_seen_current =1;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;

case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;

case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(p_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(setpoint_down_bias); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'Q':  break;
case 'S':  break;
case 'J':  break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.

}
}
if (Serial.available() >= 9){
{

case 'T': goal_v = Serial.parseFloat();break;
case 'C': p_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;

case 'J': setpoint_down_bias = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':p_gain = Serial.parseFloat();break;
case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;

}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
//  float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
//  if (get_mA() > current_cap){
//    voltage_override = 0;
//  }
//}

static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
}
unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return  1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float p_controller(float clf){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float error = clf-setpoint;
float p_term = p_gain*error*time_since_last_call_s*controller_polarity*(-1);
float new_rps = motor.target+p_term;

Serial.print("stp:");
Serial.print(setpoint);
Serial.print(",");
Serial.print("mA:");
Serial.print(get_mA());
Serial.print(",");
Serial.print("rps:");
Serial.print(new_rps);
Serial.print(",");
Serial.print("p:");
Serial.print(p_term*100);
Serial.print(",");
Serial.print("error:");
Serial.println(error);

return new_rps;
}
int detect_polarity(float clf, float rps){
static unsigned long int last_call_clock = micros();

static float last_rps = rps;
static float last_clf=clf;
static int val = 1;
static unsigned long int last_check = micros();
unsigned long int time_since_last_check = ticks_diff(micros(),last_check);
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();

if ((time_since_last_check)>300000){
last_check = micros();
float rps_diff = rps-last_rps;
last_rps = rps;
float clf_diff = clf-last_clf;
last_clf=clf;
if (rps_diff >= 0){
if (clf_diff >= 0){
val=1;
}
if (clf_diff < 0){
val=-1;
}
}
if (rps_diff < 0){
if (clf_diff > 0){
val=-1;
}
if (clf_diff <= 0){
val= 1;
}
}

}
return val;
}
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}

if (clf<lowest_seen_current){
lowest_seen_current = clf;
}
return (lowest_seen_current-setpoint_down_bias);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
driver.init();

// link the motor and the driver
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2;   // [V]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
//  Serial.println(setpoint);
Serial.println(get_mA());
}

}

//float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
//   float x =0;
//   x = currentlf_now*motor.voltage_limit/24;
//   return  1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
//}
void loop() {

static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
//     unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;

for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
controller_polarity = detect_polarity(get_mA(), motor.target);

motor.target = p_controller(get_mA());

//     overcurrent_trip();
SerialComm();
}
}
``````

Also it wavers substantially on either side of the optimal point, but that’s sort of just a matter of tuning.

Hm, couldn’t get it to work very fast, even with tuning, but the variables seem to be responding plenty fast, it’s just a matter of the optimizer. IT only accelerates and only works cw, just a quick test of the theory.

Ok this one uses more of a rollin gball thing, using the slope instead of the setpoint adjustment of a P controller :

``````#include <SimpleFOC.h>

// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.05);
LowPassFilter slope_filter = LowPassFilter(0.5);
float voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
//float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float d_gain = 0.3;// rads per second per amp
int controller_polarity = -1;
float perturb_rate = 3;//radians per second per second
float perturbation_amplitude = 1;//rads per second
//float stpt_adjust_rate = 30;//in milliamps per second
//float setpoint_down_bias = 2; // in milliiamps
//float lowest_seen_current =1;
long int perturb_cycle = 200000;// in microseconds.
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;

case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;

case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(d_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
//  case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
// case 'j': Serial.read(); Serial.print("j"); Serial.println(setpoint_down_bias); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'Q':  break;
case 'S':  break;
case 'J':  break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.

}
}
if (Serial.available() >= 9){
{

case 'T': goal_v = Serial.parseFloat();break;
case 'C': d_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;

// case 'J': setpoint_down_bias = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':d_gain = Serial.parseFloat();break;
//  case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;

}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
//  float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
//  if (get_mA() > current_cap){
//    voltage_override = 0;
//  }
//}

unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return  1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float D_controller(float mA, float rps){
static unsigned long int last_check = micros();
static unsigned long int last_call_clock = micros();
unsigned long int time_since_last_check = ticks_diff(micros(),last_check);
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
static float last_rps = rps;
static float last_mA=mA;
float time_since_last_call_s = 0;
float d_term = 0;
float rps_diff = 0;
float mA_diff = 0;
static float slope_here = 0;
float f_slope_here =0;
//  float d_term = 0;
time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
if ((time_since_last_check)>2000){
last_check = micros();
rps_diff = rps*1000-last_rps*1000;
mA_diff = (mA*1000-last_mA*1000);
slope_here = rps_diff/mA_diff;// remember to go down in rps if the slope is positive.
last_rps = rps;
last_mA = mA;
}
if (slope_here > 1){
slope_here = 1;
}
if (slope_here < -1){
slope_here = -1;
}
f_slope_here = slope_filter(slope_here);
d_term = f_slope_here*d_gain*time_since_last_call_s*controller_polarity;
rps = rps+d_term;
// diagnostic stuff
Serial.print("mA_diff:");
Serial.print(mA_diff/1000);
Serial.print(",");
Serial.print("mA:");
Serial.print(mA/100);
Serial.print(",");
Serial.print("rps:");
Serial.print(rps/100);
Serial.print(",");
Serial.print("f_slope:");
Serial.print(f_slope_here);
Serial.print(",");
Serial.print("d:");
Serial.print(d_term/time_since_last_call_s);
Serial.print(",");
Serial.print("rps_diff:");
Serial.println(rps_diff/1000);
//
return rps;
}

void perturb_rps(){
static unsigned long int last_call_clock = micros();
float time_since_last_call_s = 0;
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
static int perturbs = 0;
static float perturbation = 0;
static int perturb_dir = 1;
time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
if (perturbation > perturbation_amplitude){
perturb_dir = -1;
}
if (perturbation < 0-perturbation_amplitude){
perturb_dir = 1;
}
motor.target += perturb_rate*time_since_last_call_s*perturb_dir;
perturbation += perturb_rate*time_since_last_call_s*perturb_dir;
}
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}

void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
driver.init();

// link the motor and the driver
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2;   // [V]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();

currentlf_now = diff_filter(currentSense.getDCCurrent());
}
//  Serial.println(setpoint);
Serial.println(get_mA());
}

}

void loop() {

static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
//     unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;

for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
//    controller_polarity = detect_polarity(get_mA(), motor.target);

motor.target = D_controller(get_mA(),motor.target);
perturb_rps();
//     overcurrent_trip();
SerialComm();
}
``````

It works better in some way but there is too much noise in the current signal really. It messes things up too much. There are problems when the RPS reverses it’s direction of wandering, you could synchronize the slope measurement on that but it still looks like it’s a bit slow. There is more lag than I thought there would be, apparently. It’s possible and instant phase correction, instead of correcting the frequency, would work, to regulate things, but that would probably make noise so not an option for me.

Looks like this could be done but it is a little laggy for some reason.

I think the method used to calculate the DC current is one of the main problems. Considerable noise introduced which can be filtered but filtering it makes things laggy. You would need a current sensor at the input of the hbridge or something I think, or at least that would be better. Or maybe with the right tuning and stuff this could be gotten working.

Hm, I don’t really have time to work on this as it doesn’t appear promising with this particular board due to the stalling issue. However I think that this does indicate and important realization. The method simpleFOC uses to set the electrical angle is very stall - resistant. However the realization that most of us are not actually getting good angle regulation is valuable. A supervisory or compensation system of some kind that oversees the stall-resistant appraoch, which calculates he angle difference and compensates for communication delays and so on would yield important results I think. In the case of my system, with a relatively slow AS5600, I found that the angle difference was compressed by more than 0.5 radians due to the communication time with the sensor, at 250 rads/s, but it was not noticeable at low speeds. This leads to stall and then I tended to compensate by adding a safety margin to the PI controller setpoint.

So basically this approach doesn’t really solve the need for a fudge factor to avoid stall.

Basically both approaches are just pursuing more accurate actual regulation of the thing we are after, in the case of the current regulation it’s a more direct regulation so that’s not a bad idea, but I think it kind of needs to be combined with the approach where the angle gets set directly by drawing upon the latest sensor reading. That might be noisier though.

Oh well my angle sensor based system is working…

If I can ever quit fretting over details and get my SmoothingSensor integrated, it should theoretically be able to produce accurate results from AS5600’s PWM mode by using the timestamp from the start of the communication. If you want to try it, the changes to MagneticSensorPWM are to add a pulse_timestamp variable and this update function

``````void MagneticSensorPWM::update() {
if (!is_interrupt_based)
Sensor::update();
else
{
noInterrupts();
Sensor::update();
angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication
interrupts();
}
}
``````

And set pulse_timestamp in handlePWM where pulse_length_us is set:

``````    if (!digitalRead(pinPWM)) {
pulse_length_us = now_us - last_call_us;
pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp
}
``````

Paste the code from my last post in the thread into your .ino file SmoothingSensor: Experimental sensor angle extrapoltion - #11 by dekutree64
And add `friend class SmoothingSensor;` in the Sensor base class so it can access the protected variables.
Usage is like

``````BLDCMotor motor(args);
MagneticSensorPWM sensor(args);
SmoothingSensor smooth(&sensor, &motor);

void doPWM(){sensor.handlePWM();}

void setup() {
...
sensor.enableInterrupt(doPWM);