# New mini project: adapt the sensorless commutation sub section of odrive!(funded :)

Ok, myself and a guy I met through twitter (thanks to Sequoia who also posts here sometimes! The motor dual driver based on the RP2040), Felix Fischer, are collaborating to make a nice clean, well explained (should have a little companion document that helps with the understanding), modular addition to SimpleFOC which will give us nice sensorless commutation for SMPSM type motors!

We will adapt the code base sections of the odrive system, such as this module, which appears to be the main system that calculates motor timing from current and voltage waveform and motor characteristics:

This will be a good first step, which can be expanded to PMSM type motors, which are also common. The main difference is the â€śsalienceâ€ť has an effect on the equations, i.e. the inductance of the coils actually de facto changes with the rotational position of the rotor. This complicates things, but one foot in front of the other.

For now this will exclude the high frequency injection stuff, which allows position detection even at zero velocity, but I would love to bake that too some time, which will complement this well and may even allow a sensorless servo motor!

This will later be combined with what I hope will be the next generation flagship board for SimpleFOC, if we can agree on one, which I also plan to bake (and I have a bit of $to spend on it). It would be very similar to the QUADVRANS, just a few tweaks to allow the copper to be flooded with solder to up current capacity and maybe reduce cost etc, I donâ€™t know what current measurement sub systems that has, too (we need good stuff for sensorless of course). I personally do not require high currents, only an amp or so max actually, and people can always tweak the power stage another time. One good solid working example makes any variants much easier. This document describes the math and general strategy the Odrive takes: To be honest Iâ€™m a bit weak on the math after reading that. The little dot above the x is particularly confusing, if anyone knows what that meansâ€¦ I understand the vectors and stuff reasonably well but could certainly stand to brush up on things. I would appreciate any help anyone can give. I want this to be a â€ścontribute to the commons, draw from the commonsâ€ť paradigm undertaking. Not a â€śhack up the module I got from the commons to make something which is practically useless to anyone else and only just works even for meâ€ť, even if the result is technically open source, I believe it is important to document it well and do a clean, thorough job before anyone else can really practically use it. This is what I have many times believed companies should do. If the commons is not good enough for ones purposes, improve it! My hope is that this helps bring in expertise from others, and also forms a more solid foundation for anything I and others may need to do in the future. 2 Likes You will notice a â€śPLLâ€ť mentioned in the code. This is a phase locked loop, an oscillator that is adjusted rapidly to remain in-phase with another signal, allowing frequencies etc to be measured. I think I can probably dispense with this for now, because we have the angular frequency of the voltage, and the motor speed in my case should almost always be very close to that. This is the motor driver code, which is a bit clunky, which I am currently using. Weâ€™ll be testing this on the same board, a b-g431-esc1 board. It is just open loop with a simple approach to jack the voltage up during acceleration/deacceleration, and also ramp it up/down with RPM. Very crude, but it allowed me to prove the other systems work wrt noise etc. SimpleFOC-based-sensorless-driver-for-G-431-B-ESC1-board/README.md at main Â· adouglas89/SimpleFOC-based-sensorless-driver-for-G-431-B-ESC1-board Â· GitHub The plan is to use the approach described above to form a sub system which can measure the motor timing i.e. the angle between the magnetic field produced by the coils and the actual rotor. Then a PI control loop will regulate the angle by adjusting the voltage. This is rather unconventional but it eliminates many of the noise-producing variations in velocity etc. and allows for very predictable speed. The driver must be capable of exporting information regarding voltage, current, speed and motor timing out the UART because I need to use the fan as an anemometer to detect and regulate actual airflow. This is another story any I hope to use either curve fitting or machine learning micropython libraries to do this on the pico W. That will be the repository for this project, to weave in the actual commutation sub-system. Does anyone remember how to get the exact voltage that Simpelfoc is supposed to be putting out on phase A and B at a given moment? The little dot above a function is used here to mean the derivative of the function, Iâ€™m pretty sure 1 Like Hey, It would be driver.dc_a * driver.voltage_power_supply (Or dc_b or dc_c for the other two phases) 1 Like thx, interesting, never seen a dot like that before that I can remember. In physics it usually means the time derivative and saves a lot of typing. Physicists are often very pragmatic . Example for velocity, instead of typing v(t) = ds/dt you type: v(t) = s(t) dot or even shorter v(t) = s dot. For the second derivative you use two dots, so for acceleration: a(t) = s dot dot. Pretty neat actuallyâ€¦ 1 Like And in case anyone is interested, when talking about math, the forum supports embedding Latex style maths notation, so you can also write: v(t) = \frac{ds}{dt} and v(t) = \dot{s}(t) = \dot{s} a(t) = \ddot{s} by embedding the latex between two $ characters, like this:

$v(t) = \frac{ds}{dt}$ is rendered as v(t) = \frac{ds}{dt}

2 Likes

Hey guys, Iâ€™m having difficulty getting a solid voltage reading. The voltage is floating around up there. Shouldnâ€™t the sine waves kind of be centered around zero volts, or at least their troughs should touch zero, depending on the convention used?

full program in case it is relevant :

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 0;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
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 Va = 0;
float Vb = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).

// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.

// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// Pll gains as a function of bandwidth
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
static float V_alpha_beta_memory_[] = {0,0};
static float flux_state_[] = {0,0};
static float last_v_alpha = 0;
static float last_v_beta = 0;
float  observer_gain = 1000; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
float O_phase_resistance = 2.9;// in ohms *presumably
float O_phase_inductance = 0.0006455;  // in henries *presumably
float O_flux_linkage = 0.009310406047450554;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = millis();
unsigned long ms_since_last_run = ticks_diff(last_run_ts, millis()) ;
float s_since_last_run = float(ms_since_last_run)/1000 ;
float current_meas_period = s_since_last_run ;
// Clarke transform
float one_by_sqrt3 = 0.5773502691;
float I_alpha_beta[2] = {
current_A,
one_by_sqrt3 * (current_B - (-current_B-current_A))};

// alpha-beta vector operations
float eta[2];
for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = y;
// integrate prediction to current timestep
flux_state_[i] += x_dot * current_meas_period;

// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}

// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = O_flux_linkage * O_flux_linkage;
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
float bandwidth_factor = 1.0f / pm_flux_sqr;
float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
float x_dot = eta_factor * eta[i];
// convert action to discrete-time
flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}

// Flux state estimation done, store V_alpha_beta for next timestep

V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no

return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
// driver.dead_zone = 0.1;
driver.init();
// driver.dead_zone = 0.1;
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

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;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}

if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;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();
}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
//Va = driver.dc_a;
//Vb = driver.dc_b;
Serial.print("A:"); Serial.print(A);
Serial.print(",B:"); Serial.print(B);
Serial.print(",Va:"); Serial.print(Va);
Serial.print(",Vb:"); Serial.println(Vb);
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}


On the plus side, it does not appear that I have to zero the current reading sub-system. I seem to remember that was a thing?

BTW I feel I owe Candas1 an apology, I cannot understand how your code works and thus chose to focus on the odrive codeâ€¦ I suck, sorry :(.

update: I adapted the Canadas1 code segment and am trying it now:

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 0;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
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 Va = 0;
float Vb = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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 observer(float A, float B, float Va, float Vb){
static unsigned long observer_timestamp = 0;
float Ts = ( _micros() - observer_timestamp) * 1e-6f;
static float ABCurrent_prev;
static float flux_a, flux_b;
float flux_linkage = 0.009310406047450554;
static float ABCurrent_prev_a = 0;
static float ABCurrent_prev_b = 0;
flux_a = _constrain( flux_a + (Va - motor.phase_resistance * Va) * Ts -
motor.phase_inductance * (A - ABCurrent_prev_a),-flux_linkage, flux_linkage);
flux_b = _constrain(flux_b + (Vb - motor.phase_resistance * Vb) * Ts -
motor.phase_inductance * (B - ABCurrent_prev_b) ,-flux_linkage, flux_linkage);
ABCurrent_prev_a = A;
ABCurrent_prev_b = B;
observer_timestamp = _micros();

float tmp_angle_el = atan2(flux_b,flux_a);
// Handle wraparound
tmp_angle_el += tmp_angle_el < 0 ? _2PI : ( tmp_angle_el >= _2PI ? -_2PI : 0 );
float angle_el = tmp_angle_el;
return angle_el;
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
// driver.dead_zone = 0.1;
driver.init();
// driver.dead_zone = 0.1;
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;

//sensorless stuff

motor.phase_resistance = 2.9;
motor.phase_inductance = 0.0006455;
}

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;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}

if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;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();
}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}

//start sensorless observer stuff
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
//Va = driver.dc_a;
//Vb = driver.dc_b;
//Serial.print("A:"); Serial.print(A);
//Serial.print(",B:"); Serial.print(B);
//Serial.print(",Va:"); Serial.print(Va);
//Serial.print(",Vb:"); Serial.println(Vb);
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
Serial.println(observer(A,B,Va,Vb));

}

update: no it's just a flatline, better to back to the odrive stuff...

update, with the code below, I am stuck because somehow the floating point data type overflows when I multiply even relatively small numbers by each other.

The output in serial monitor is:


19:20:54.097 -> ,y:-0.13,fs:0.00I_alpha_beta[i]0.05eta[i]-0.00,y:-0.09,fs:0.00I_alpha_beta[i]0.03eta[i]-0.000.000.00est_pm_flux_sqr:0.00,eta[1]:-0.00,eta[0]:-0.00
19:20:54.097 -> ,y:0.12,fs:534025.31I_alpha_beta[i]-0.04eta[i]534025.31,y:0.06,fs:237685.88I_alpha_beta[i]-0.02eta[i]237685.88ovfovfest_pm_flux_sqr:ovf,eta[1]:ovf,eta[0]:ovf



Iâ€™m printing the variables and expressions as they are calculated to debug things. You see the order of the execution and the print statements in the code below. The error first arises on the line where it says " double est_pm_flux_sqr = (pow(eta[0],2)) + pow(eta[1],2);". Iâ€™ve tried several variations, but basically it thinks
534025
multiplied by 237685 is an overflow. I tried changing the type to double i.e. double precision floating point but nothing improved for jack. Maybe there is something wrong with the floating point on this system? :

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 1;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
float current_limit_slope = 1.8;// 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 Va = 0;
float Vb = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).

// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.

// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// Pll gains as a function of bandwidth
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
static float V_alpha_beta_memory_[] = {0,0};
static double flux_state_[] = {0,0};
static float last_v_alpha = 0;
static float last_v_beta = 0;
float observer_gain = 200; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
float O_phase_resistance = 2.9;// in ohms *presumably
float O_phase_inductance = 0.0006455;  // in henries *presumably
float O_flux_linkage = 0.00931;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = millis();
unsigned long ms_since_last_run = ticks_diff(last_run_ts, millis()) ;
float s_since_last_run = float(ms_since_last_run)/1000 ;
float current_meas_period = s_since_last_run ;
// Clarke transform
float one_by_sqrt3 = 0.5773502691;
float I_alpha_beta[2] = {
current_A,
one_by_sqrt3 * (current_B - (-current_B-current_A))};

// alpha-beta vector operations
double eta[2] = {0,0};
if (get_mA()<100){ // if current measurement is invalid
flux_state_[0] = 0;
flux_state_[1] = 0;
}
for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
Serial.print(",y:");
Serial.print(y);
// flux dynamics (prediction)
float x_dot = y;
// integrate prediction to current timestep
flux_state_[i] += x_dot * current_meas_period;
Serial.print(",fs:");
Serial.print(flux_state_[i]);
// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
Serial.print("I_alpha_beta[i]");
Serial.print(I_alpha_beta[i]);
Serial.print("eta[i]");
Serial.print(eta[i]);
}

// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = O_flux_linkage * O_flux_linkage;
Serial.print(pow(eta[1],2));
Serial.print(pow(eta[1],2));
double est_pm_flux_sqr = (pow(eta[0],2)) + pow(eta[1],2);
double bandwidth_factor = 1 / pm_flux_sqr;
double eta_factor = 0.5 * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

Serial.print("est_pm_flux_sqr:");
Serial.print(est_pm_flux_sqr);
// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
double x_dot = eta_factor * eta[i];
// convert action to discrete-time

flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}

// Flux state estimation done, store V_alpha_beta for next timestep

V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
last_run_ts = millis();
Serial.print(",eta[1]:");
Serial.print(eta[1]);
Serial.print(",eta[0]:");
Serial.println(eta[0]);
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no

return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
// driver.dead_zone = 0.1;
driver.init();
// driver.dead_zone = 0.1;
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

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;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}

if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;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();
}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
//Va = driver.dc_a;
//Vb = driver.dc_b;
// Serial.print("A:"); Serial.print(A);
//Serial.print(",B:"); Serial.print(B);
// Serial.print(",Va:"); Serial.print(Va);
// Serial.print(",Vb:"); Serial.print(Vb);

track_return_motor_timing(Va, Vb, A, B);
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}


Yeah, double is usually just an alias for float on Arduino. But that should be fine. You just lose accuracy of the lower digits on large numbers, but you shouldnâ€™t get an overflow. Is the problem specific to the pow function, or any multiplication? If youâ€™re using literals, include .0 and/or f at the end (e.g. 534025f * 237685f), otherwise it will do the multiply as integers first, which does overflow 32 bits, and then convert to float. I think the behavior for that is technically undefined, so it could convert the truncated result or set the float to overflowed.

1 Like

Yeah I think itâ€™s some kind of conversion to int happening somewhere like you indicateâ€¦ will try putting f s after stuff.

I tried things like eta[0]*eta[0] instead of pow(eta[0],2)

even if I go
Serial.print((float(eta[0]) * float(eta[0])));

It still says itâ€™s an overflow!

Could you try a few things for me? I wanna check these, but I donâ€™t know how to reproduce your environment locally.

1. Serial.print((float(eta[0])));
2. Serial.print(eta[0]);
3. Serial.print(flux_state_[0]);
4. Serial.print(0_phase_inductance * I_alpha_beta[0]);

Yeah, thatâ€™s definitely weird. Reading through the code more carefully, I donâ€™t see anything that should result in large int*int multiplication. Usually if one operand is a floating point type and the other is integer, the int will be converted to float before the operation. So things like double bandwidth_factor = 1 / pm_flux_sqr; should be ok since pm_flux_sqr is float. It may indeed be a bad software float implementation.

I noticed when eta[0] and eta[1] are printed, in includes a .00 after the number, indicating itâ€™s a floating point, but itâ€™s always .00. The digits after the decimal point are always zero after the first time or two it prints.

Itâ€™ll probably turn out to be some amateur mistake on my part or something, but this is why I donâ€™t like arduinoâ€¦ Iâ€™ll do the data collection Felix suggests

ok here is the output of the program below

11:01:11.936 -> test serial2
11:01:15.044 -> ,flux_state_[i]:0.00,O_phase_inductance * float(I_alpha_beta[i]-0.00,eta[i]0.00,float(eta[i])0.00,flux_state_[i]:0.00,O_phase_inductance * float(I_alpha_beta[i]0.00,eta[i]-0.00,float(eta[i])-0.00(eta[1] * eta[1])0.00est_pm_flux_sqr:0.00,eta[1]:-0.00,eta[0]:0.00
11:01:15.044 -> ,flux_state_[i]:1434208.62,O_phase_inductance * float(I_alpha_beta[i]-0.00,eta[i]1434208.63,float(eta[i])1434208.62,flux_state_[i]:832041.13,O_phase_inductance * float(I_alpha_beta[i]-0.00,eta[i]832041.13,float(eta[i])832041.13(eta[1] * eta[1])ovfest_pm_flux_sqr:ovf,eta[1]:ovf,eta[0]:ovf
11:01:15.044 -> ,flux_state_[i]:51144548.00,O_phase_inductance * float(I_alpha_beta[i]0.00,eta[i]51144548.00,float(eta[i])51144548.00,flux_state_[i]:56894844.00,O_phase_inductance * float(I_alpha_beta[i]0.00,eta[i]56894844.00,float(eta[i])56894844.00(eta[1] * eta[1])ovfest_pm_flux_sqr:ovf,eta[1]:ovf,eta[0]:ovf
11:01:15.044 -> ,flux_state_[i]:51202176.00,O_phase_inductance * float(I_alpha_beta[i]0.00,eta[i]51202176.00,float(eta[i])51202176.00,flux_state_[i]:57318528.00,O_phase_inductance * float(I_alpha_beta[i]0.00,eta[i]57318528.00,float(eta[i])57318528.00(eta[1] * eta[1])ovfest_pm_flux_sqr:ovf,eta[1]:ovf,eta[0]:ovf
11:01:15.044 -> ,flux_state_[i]:51611404.00,O_phase_inductance * float(I_alpha_beta[i]-0.00,eta[i]51611404.00,float(eta[i])51611404.00,flux_state_[i]:56505832.00,O_phase_inductance * float(I_alpha_beta[i]-0.00,eta[i]56505832.00,float(eta[i])56505832.00(eta[1] * eta[1])ovfest_pm_flux_sqr:ovf,eta[1]:ovf,eta[0]:ovf


code:

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 1;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
float current_limit_slope = 1.8;// 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 Va = 0;
float Vb = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).

// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.

// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// Pll gains as a function of bandwidth
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
static float V_alpha_beta_memory_[] = {0.0f,0.0f};
static double flux_state_[] = {0.0f,0.0f};
static float last_v_alpha = 0.0f;
static float last_v_beta = 0.0f;
float observer_gain = 200.0f; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
float O_phase_resistance = 2.9f;// in ohms *presumably
float O_phase_inductance = 0.0006455f;  // in henries *presumably
float O_flux_linkage = 0.00931f;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = millis();
unsigned long ms_since_last_run = ticks_diff(last_run_ts, millis()) ;
float s_since_last_run = float(ms_since_last_run)/1000.0f ;
float current_meas_period = s_since_last_run ;
// Clarke transform
float one_by_sqrt3 = 0.5773502691f;
float I_alpha_beta[2] = {
current_A,
one_by_sqrt3 * (current_B - (-current_B-current_A))};

// alpha-beta vector operations
double eta[2] = {0.0f,0.0f};
if (get_mA()<100){ // if current measurement is invalid
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
}
for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = float(y);
// integrate prediction to current timestep
flux_state_[i] += x_dot * float(current_meas_period);

// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * float(I_alpha_beta[i]);

Serial.print(",flux_state_[i]:");
Serial.print(flux_state_[i]);
Serial.print(",O_phase_inductance * float(I_alpha_beta[i]");
Serial.print(O_phase_inductance * float(I_alpha_beta[i]));
Serial.print(",eta[i]");
Serial.print(eta[i]);
Serial.print(",float(eta[i])");
Serial.print(float(eta[i]));
}

// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = O_flux_linkage * O_flux_linkage;

Serial.print("(eta[1] * eta[1])");
Serial.print((eta[1] * eta[1]));
double est_pm_flux_sqr = (eta[0] * eta[0]) + (eta[1] * eta[1]);
double bandwidth_factor = 1 / pm_flux_sqr;
double eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

Serial.print("est_pm_flux_sqr:");
Serial.print(est_pm_flux_sqr);
// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
double x_dot = eta_factor * eta[i];
// convert action to discrete-time

flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}

// Flux state estimation done, store V_alpha_beta for next timestep

V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
last_run_ts = millis();
Serial.print(",eta[1]:");
Serial.print(eta[1]);
Serial.print(",eta[0]:");
Serial.println(eta[0]);
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no

return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
// driver.dead_zone = 0.1;
driver.init();
// driver.dead_zone = 0.1;
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

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;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}

if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;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();
}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
//Va = driver.dc_a;
//Vb = driver.dc_b;
// Serial.print("A:"); Serial.print(A);
//Serial.print(",B:"); Serial.print(B);
// Serial.print(",Va:"); Serial.print(Va);
// Serial.print(",Vb:"); Serial.print(Vb);

track_return_motor_timing(Va, Vb, A, B);
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}


if I go
Serial.print(â€ś(832041.13f * 1434208.63f)â€ť);
Serial.print((832041.13f * 1434208.63f));
The second line says ovf. so fucked.

Ok I think it might be a defect in the print function because Serial.print((832041.13f * 1434208.63f)/100000); does actually return a result. So maybe evertying is working fine, print just canâ€™t print it. Scoff. They spend time changing all kinds of stuff before they get the basics like this wroking reliablyâ€¦

ok, update. I assumed it was just a problem with the print function and tried to carry on.

The following lines are printed when the motor starts to accelerate from the default start of 2 radians per second, commanded to reach 60 rads per second:

11:46:35.934 -> ,eta[i]64614040.00,eta[i]55865640.00eta_factor:ovfeta[i]:64614040.00x_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovfeta_factor:ovfeta[i]:55865640.00x_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovf,eta[1]:ovf,eta[0]:ovf
11:46:35.934 -> phase:-2.43
11:46:35.934 -> ,eta[i]43315504.00,eta[i]66788448.00eta_factor:ovfeta[i]:43315504.00x_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovfeta_factor:ovfeta[i]:66788448.00x_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovf,eta[1]:ovf,eta[0]:ovf
11:46:35.934 -> phase:-2.15
11:46:35.934 -> ,eta[i]ovf,eta[i]ovfeta_factor:ovfeta[i]:ovfx_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovfeta_factor:ovfeta[i]:ovfx_dot:ovfcurrent_meas_period:4294967.50flux_state_[i]:ovf,eta[i]ovf,eta[1]:ovf,eta[0]:ovf
11:46:35.934 -> phase:1.00
11:46:35.934 -> ,eta[i]ovf,eta[i]ovfeta_factor:ovfeta[i]:ovfx_dot:infcurrent_meas_period:4294967.50flux_state_[i]:inf,eta[i]infeta_factor:ovfeta[i]:ovfx_dot:infcurrent_meas_period:4294967.50flux_state_[i]:inf,eta[i]inf,eta[1]:inf,eta[0]:inf
11:46:35.934 -> phase:-2.36
11:46:35.934 -> ,eta[i]inf,eta[i]infeta_factor:infeta[i]:infx_dot:infcurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]naneta_factor:infeta[i]:infx_dot:infcurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]nan,eta[1]:nan,eta[0]:nan
11:46:35.934 -> phase:nan
11:46:35.934 -> ,eta[i]nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]nan,eta[1]:nan,eta[0]:nan
11:46:35.934 -> phase:nan
11:46:35.934 -> ,eta[i]nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]nan,eta[1]:nan,eta[0]:nan
11:46:35.934 -> phase:nan
11:46:35.934 -> ,eta[i]nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]nan,eta[1]:nan,eta[0]:nan
11:46:35.934 -> phase:nan
11:46:35.934 -> ,eta[i]nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]naneta_factor:naneta[i]:nanx_dot:nancurrent_meas_period:4294967.50flux_state_[i]:nan,eta[i]nan,eta[1]:nan,eta[0]:nan
11:46:36.071 -> phase:nan



The exact code Iâ€™m using is:

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 1;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
float current_limit_slope = 1.8;// 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 Va = 0;
float Vb = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).

// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.

// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// Pll gains as a function of bandwidth
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
static float V_alpha_beta_memory_[] = {0.0f,0.0f};
static double flux_state_[] = {0.0f,0.0f};
static float last_v_alpha = 0.0f;
static float last_v_beta = 0.0f;
float observer_gain = 200.0f; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
float O_phase_resistance = 2.9f;// in ohms *presumably
float O_phase_inductance = 0.0006455f;  // in henries *presumably
float O_flux_linkage = 0.00931f;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = millis();
unsigned long ms_since_last_run = ticks_diff(last_run_ts, millis()) ;
float s_since_last_run = float(ms_since_last_run)/1000.0f ;
float current_meas_period = s_since_last_run ;
// Clarke transform
float one_by_sqrt3 = 0.5773502691f;
float I_alpha_beta[2] = {
current_A,
one_by_sqrt3 * (current_B - (-current_B-current_A))};

// alpha-beta vector operations
double eta[2] = {0.0f,0.0f};
if (get_mA()<100){ // if current measurement is invalid
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
}
for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = float(y);
// integrate prediction to current timestep
flux_state_[i] += x_dot * float(current_meas_period);

// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * float(I_alpha_beta[i]);

Serial.print(",eta[i]");
Serial.print(eta[i]);
}

// Non-linear observer (see paper eqn 8):
double pm_flux_sqr = O_flux_linkage * O_flux_linkage;

double est_pm_flux_sqr = (eta[0] * eta[0]) + (eta[1] * eta[1]);
double bandwidth_factor = 1 / pm_flux_sqr;
double eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
double x_dot = eta_factor * eta[i];
// convert action to discrete-time
Serial.print("eta_factor:");
Serial.print(eta_factor);
Serial.print("eta[i]:");
Serial.print(eta[i]);
Serial.print("x_dot:");
Serial.print(x_dot);
flux_state_[i] += x_dot * current_meas_period;
// update new eta

Serial.print("current_meas_period:");
Serial.print(current_meas_period);
Serial.print("flux_state_[i]:");
Serial.print(flux_state_[i]);
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
Serial.print(",eta[i]");
Serial.print(eta[i]);
}

// Flux state estimation done, store V_alpha_beta for next timestep

V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
last_run_ts = millis();
Serial.print(",eta[1]:");
Serial.print(eta[1]);
Serial.print(",eta[0]:");
Serial.println(eta[0]);
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no
Serial.print("phase:");
Serial.println(phase);
return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]

driver.voltage_power_supply = 24;
// driver.dead_zone = 0.1;
driver.init();
// driver.dead_zone = 0.1;
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]

motor.controller = MotionControlType::velocity_openloop;

// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

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;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}

if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;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();
}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
//Va = driver.dc_a;
//Vb = driver.dc_b;
// Serial.print("A:"); Serial.print(A);
//Serial.print(",B:"); Serial.print(B);
// Serial.print(",Va:"); Serial.print(Va);
// Serial.print(",Vb:"); Serial.print(Vb);

track_return_motor_timing(Va, Vb, A, B);
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}


Same as before, just print statements in different places. If this keeps up much longer Iâ€™ll have to learn to use the debugging features of arduino, I guess, but IDK if they really give you a record of exactly what happened. Print is more flexible in some ways, just kind of slow to explore possibilities.

So basically the first time inf appears, which then propagates down and turns into a nan, is when two numbers eta_factor and eta[0] are multiplied by each other. They are both floating point, probably double precision floating point. I canâ€™t see their values because print canâ€™t print them. But multiplying two perfectly good floating point values should never really give an inf???

Well felix and I got rid of most of the errors, basically the numbers were getting ridiculously large for various reasons.

This gives something that kind of looks like something on the serial plotter, but it does not change when I change the power supply voltage, so definitely more to doâ€¦ I think maybe the observer is not running with a high enough frequency. Also I need to calibrate/zero the ADC/current measurement subsystem of the board

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 1;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
float current_limit_slope = 1.8;// 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 Va = 0;
float Vb = 0;

void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).
// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
//the basis of the code here is just dumbly adapted from the odrive implementation, with the removal of unnecessary features like the pll for speed tracking.
static float V_alpha_beta_memory_[] = {0.0f,0.0f};
static double flux_state_[] = {0.0f,0.0f};
static float last_v_alpha = 0.0f;
static float last_v_beta = 0.0f;
static float observer_gain = 200.0f; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
static float O_phase_resistance = 2.9f;// in ohms *presumably
static float O_phase_inductance = 0.0006455f;  // in henries *presumably
static float O_flux_linkage = 0.00931f;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = micros();
unsigned long mcs_since_last_run = ticks_diff(micros(),last_run_ts) ;
float s_since_last_run = float(mcs_since_last_run)/1000000.0f ;
float current_meas_period = s_since_last_run ;
static float one_by_sqrt3 = 0.5773502691f;

if ((current_meas_period * 1000 > 1.0f)) { // what should the coefficient be here? Just run it really fast for now. you can't because you have to be able to debug it.  well ten times per oscillation should be fine, 60 rads per second test speed is 67 hz.  so at least every 1/670 seconds
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
V_alpha_beta_memory_[0] = 0.0f;
V_alpha_beta_memory_[1] = 0.0f;
Serial.print("cmp too long:");
Serial.println(current_meas_period*1000000);
last_run_ts = micros();
return false;
}
if (get_mA()<100){ //assume measurement is invalid
current_A = 0.0f;
current_B = 0.0f;
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
V_alpha_beta_memory_[0] = 0.0f;
V_alpha_beta_memory_[1] = 0.0f;
//Serial.print("current too low:");
//Serial.println(get_mA());
last_run_ts = micros();
return false;
}
float I_alpha_beta[2] = {
current_A,
one_by_sqrt3 * (current_B - (-current_B-current_A))};

// alpha-beta vector operations
double eta[2] = {0.0f,0.0f};

for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = float(y);
// integrate prediction to current timestep
flux_state_[i] += x_dot * float(current_meas_period);
// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * float(I_alpha_beta[i]);
//        Serial.print(",eta[i]");
//        Serial.print(eta[i]);
}

// Non-linear observer (see paper eqn 8):
double pm_flux_sqr = O_flux_linkage * O_flux_linkage;

double est_pm_flux_sqr = (eta[0] * eta[0]) + (eta[1] * eta[1]);
double bandwidth_factor = 1 / pm_flux_sqr;
//     Serial.print("log est_pm_flux_sqr:");
//     Serial.print(log10(est_pm_flux_sqr));
//     Serial.print("log eta[0]:");
//     Serial.print(log10(eta[0]));
//     Serial.print("log eta[1]:");
//     Serial.print(log10(eta[1]));
double eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
double x_dot = eta_factor * eta[i];
// convert action to discrete-time
//     Serial.print("eta_factor:");
//     Serial.print(log10(eta_factor));
//     Serial.print("eta[i]:");
//     Serial.print(log10(eta[i]));
//     Serial.print("x_dot:");
//     Serial.print(x_dot);
//     Serial.print("cmp:");
//     Serial.print(current_meas_period);
flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}
// Flux state estimation done, store V_alpha_beta for next timestep
V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
last_run_ts = micros();
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no
//    Serial.print("phase:");
//    Serial.println(phase);
return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

float run_observer(){
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
return track_return_motor_timing(Va, Vb, A, B);
}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;

run_observer();

loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}
if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
run_observer();
motor.move();
motor.move();
motor.move();

run_observer();

}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}

Serial.println(run_observer());
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}


Yeah, just checked it, it doesnâ€™t change at all even when I run it at 100 rads/second and change the power supply voltage from 24 to 21 volts, so pretty serious issues still, even after adc is calibrated thatâ€™s not going to be enough, there is something else wrong.

update:
ok the â€śphaseâ€ť is supposed to be the angle of the mangetic field produced by the magnets on the rotor, not the motor timing. However itâ€™s still not working, the frequency goes down rather than up, probably because the observer is not running fast enough/frequently enough, but this could be an artifact of the serial monitor in arduino. . I need to solve the issue with the voltages floating around above zero and also get the current readings to be more accurate maybe by doing some kind of adc calibration or zeroing thing.

Probably I will try to solve the voltage signal accuracy and current signal accuracy, maybe make sure itâ€™s running the observer more often, maybe see how often and then just subtract the the phase from the atan2 of the clarke transform of the current of two of the phases (so angular rotor position within a cycle minus the position of the field produced by the stator within a cycle), then see if that value, which should be the motor timing, changes when I adjust the voltage etc.

It most certainly can. Not only that, but if you donâ€™t take great care in the design of your numerical algorithms, you will ruin the precision of your calculations, to the point that the output is meaningless.

A simple example:

32 bit floats can encode numbers in the following range, so these represent the biggest numbers that can be represented:

std::numeric_limits<float>::lowest() == -FLT_MAX == -3.40282e+38
std::numeric_limits<float>::highest() == FLT_MAX == 3.40282e+38


but the tiniest number that can be encoded is:

std::numeric_limits<float>::min() == 1.17549e-38


Whichever number you choose to represent, it gets represented in 32 bits, of which 8 are for the exponent, 23 for the â€śmantissaâ€ť (digits) and 1 bit encodes the sign Â±.

So letâ€™s say you take a simple number like 1e30, and you try to add 1 to it. What happens? As it turns out, in floats, 1e30 + 1.0f == 1e30.

Why?

Because when the float is representing 1e30, it does not have enough bits to represent single digits like 1.

The last bit of the mantissa represents 1/8388608 * 2^99, which is still an absolutely huge number. You can try it outâ€¦ even if you add 10000 or 100000 to 1e30 as floats, youâ€™ll get 1e30â€¦

So, keep in mind when coding numerical algorithms that you always have to be aware of the range of the calculations, and the order of operations can make a huge differenceâ€¦

(big number + small number) / number = COULD BE WRONG
(big number / number) + (small number / number) = MORE LIKELY TO BE RIGHT
Edit - not actually sure this example is complicated enough, but you get the idea.

If interested in numeric algorithms, this reference is highly recommended: https://www.amazon.com/Numerical-Recipes-Scientific-Computing-Second/dp/0521431085

1 Like

thanks, Runger, by printing the log10() of the number we were able to ascertain the problem is indeed as you indicate. The numbers were actually exceeding 10^308 lol. This was the clue, I was able to get in there and fix it. The code below appears to give a reasonable approximation of the rotor angle, or rather the position in radians along the cycle of the changing magnetic flux caused by the magnets on the rotor, to be precise.

The next step will be to get it to give me the actual motor timing angle, and then to calibrate or at least zero the ADC/current measurement sub-system.

Then the concern will be if it is fast enough to operate without causing a buzzing sound, that might be an issue, if that happens I can just sprinkle motor.move() commands in there, bit crude but gotta make do.

#include <SimpleFOC.h>
#include <math.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);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
float goal_speed =0;
float v=2;
float v_diff=1;
float accel = 92;// in rads per second per second
float v_per_radsPS = 0.0232;
float accel_v_boost = 0.5;// voltage is increased during acceleration and deacceleration by this amount
bool voltage_override = 1;
float power_figure = 1.5;
float power_coeff = 0.00043;// the serial communicator could actually use an extra digit for this one.
float A, B, C;
float currentlf_now =0;
float prop_V= 0;
float min_V = 1;
float v_limit = 19;
float current_limit_slope = 1.8;// 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 Va = 0;
float Vb = 0;
float motortiming = 0;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_speed); break;
case 'c': Serial.read(); Serial.print("c"); Serial.println(accel); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit, 4); break;
case 'n': Serial.read(); Serial.print("n"); Serial.println(v_diff); break;
case 'p': Serial.read(); Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'b': Serial.read(); Serial.print("b"); Serial.println(accel_v_boost); 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 'f': Serial.read(); Serial.print("f"); Serial.println(power_coeff, 6); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent(), 5); break;
case 'i': Serial.read(); Serial.print("i"); Serial.println(get_mA(), 4); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(min_V); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'k': Serial.read(); Serial.print("k"); Serial.println(v_limit); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); 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 'V':  break;
case 'P':  break;
case 'B':  break;
case 'Y':  break;
case 'U':  break;
case 'O': break;
case 'F':  break;
case 'J':  break;
case 'W': ;break;
case 'K': ;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){
switch(Serial.read())
{

case 'T': goal_speed = Serial.parseFloat();break;
case 'C': accel = Serial.parseFloat();break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'K': v_limit = Serial.parseFloat(); break;
case 'B': accel_v_boost = 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': power_coeff = Serial.parseFloat();break;
// case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
// case 'J': min_V = 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;
}
}
float track_return_motor_timing(float v_pA, float v_pB, float current_A, float current_B, float current_C) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).
// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
// don't engage the system by calling this function until a suitable RPM is achieved or yu will just get nonsense results
//the basis of the code here is just dumbly adapted from the odrive implementation, with the removal of unnecessary features like the pll for speed tracking.
static float V_alpha_beta_memory_[] = {0.0f,0.0f};
static double flux_state_[] = {0.0f,0.0f};
static float last_v_alpha = 0.0f;
static float last_v_beta = 0.0f;
static float observer_gain = 10.0f; // IDK what a reasonable starting point is, 1000 is what odrive uses. They use 2500 and 8000 in the pdf doc.  Too high and it will bounce around, too low and it will take a long time to catch up with changes.
static float O_phase_resistance = 2.9f;// in ohms *presumably
static float O_phase_inductance = 0.0006455f;  // in henries *presumably
static float O_flux_linkage = 0.00931f;// it's in units of webers *presumably, which is ampere-turns? Can be calculated from the peak to peak per hz, which is easy to measure, so find the equation for that and use that to calc it before then use that.*is this per phase or across terminals*??? I measured it across the terminals
static unsigned long last_run_ts = micros();
unsigned long mcs_since_last_run = ticks_diff(micros(),last_run_ts) ;
float s_since_last_run = float(mcs_since_last_run)/1000000.0f ;
float current_meas_period = s_since_last_run ;
static float one_by_sqrt3 = 0.5773502691f;

if ((current_meas_period * 1000 > 1.0f)) { // what should the coefficient be here? Just run it really fast for now. you can't because you have to be able to debug it.  well ten times per oscillation should be fine, 60 rads per second test speed is 67 hz.  so at least every 1/670 seconds
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
V_alpha_beta_memory_[0] = 0.0f;
V_alpha_beta_memory_[1] = 0.0f;
Serial.print("cmp too long:");
Serial.println(current_meas_period*1000000);
last_run_ts = micros();
return false;
}
if (get_mA()<70){ //assume measurement is invalid
current_A = 0.0f;
current_B = 0.0f;
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
V_alpha_beta_memory_[0] = 0.0f;
V_alpha_beta_memory_[1] = 0.0f;
//Serial.print("current too low:");
//Serial.println(get_mA());
last_run_ts = micros();
return false;
}
float I_alpha_beta[2] = {current_A, one_by_sqrt3 * (current_B -current_C)};

// alpha-beta vector operations
double eta[2] = {0.0f,0.0f};

for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -O_phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = float(y);
// integrate prediction to current timestep
flux_state_[i] += x_dot * float(current_meas_period);
// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - O_phase_inductance * float(I_alpha_beta[i]);
//        Serial.print(",eta[i]");
//        Serial.print(eta[i]);
}

// Non-linear observer (see paper eqn 8):
double pm_flux_sqr = O_flux_linkage * O_flux_linkage;

double est_pm_flux_sqr = (eta[0] * eta[0]) + (eta[1] * eta[1]);
double bandwidth_factor = 1 / pm_flux_sqr;
//     Serial.print("log est_pm_flux_sqr:");
//     Serial.print(log10(est_pm_flux_sqr));
//     Serial.print("log eta[0]:");
//     Serial.print(log10(eta[0]));
//     Serial.print("log eta[1]:");
//     Serial.print(log10(eta[1]));
double eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
double x_dot = eta_factor * eta[i];
// convert action to discrete-time
//     Serial.print("eta_factor:");
//     Serial.print(log10(eta_factor));
//     Serial.print("eta[i]:");
//     Serial.print(log10(eta[i]));
//     Serial.print("x_dot:");
//     Serial.print(x_dot);
//     Serial.print("cmp:");
//     Serial.print(current_meas_period);
flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - O_phase_inductance * I_alpha_beta[i];
}
// Flux state estimation done, store V_alpha_beta for next timestep
V_alpha_beta_memory_[0] = last_v_alpha; // still have to figure out exactly what voltage this is so we can sub in the right one
V_alpha_beta_memory_[1] = last_v_beta; // same here
last_v_alpha = v_pA;
last_v_beta = v_pB;
last_run_ts = micros();
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no
//return I_alpha_beta[1];
return phase;  // the "phase" is the motor timing, in radians presumably, optimal for torque and efficiency is of course pi/2 radians but we need some space to avoid stall too
};

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.
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 1;   // [V]
motor.velocity_limit = 300; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}

float run_observer(){
current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
C = current1.c;
Va = driver.dc_a * driver.voltage_power_supply;
Vb = driver.dc_b * driver.voltage_power_supply;
return track_return_motor_timing(Va, Vb, A, B, C);
}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;

//run_observer();

loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;
if (motor.target < goal_speed-(accel*loop_time_s*1.5)){//rps not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
}
if (motor.target>=goal_speed-(accel*loop_time_s*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*loop_time_s*1.5)){
motor.move();
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override; //constant run
}
}

if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
motor.move();
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS))+(power_coeff*pow(fabs(motor.target),power_figure)))*voltage_override;
}

}
if (prop_V < min_V){
motor.voltage_limit = min_V*voltage_override;
}
else {
motor.voltage_limit = prop_V;
}
if (prop_V > v_limit){
motor.voltage_limit = v_limit;
}

for (int i=0;i<10;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
//run_observer();
motor.move();
motor.move();
motor.move();

motortiming = run_observer();

}

//     Serial.println(micros()-inner_loop_time);
//     inner_loop_time = micros();
SerialComm();
}

Serial.println(motortiming);

currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
overcurrent_trip();
}
`

btw the double seems to work, we do actually get double precision floating point which is kind of cool