This system is a crude open loop drive system with some acceleration and voltage ramping provisions to prevent stall and drive the motor in a range where it won’t overheat, just for testing to determine if the observer is working/to get it working.
The observer takes the voltages and currents and if run often enough is supposed to track/calculate eta[0] and eta[1]. Atan of these vectors is supposed to give the rotor position, within a cycle of commutation i.e. 1/7 of a rotation for a 7 pole pair motor, supposedly.
It does not quite work, it should ramp up linearly with time and then repeat, if the motor is turning smoothly and constantly. however it is more of an S shape, it’s distorted for some reason. I think this is probably because the observer does not factor in saliency, which is the variation of inductance of the coils as a function of rotor position. The salience of the motor is also why, according to tech support, the Texas instruments 8316A driver is incapable of driving this motor. It has bad acceleration anyway, though.
The relative position of the rotor and the magnetic field produced by the coils is supposed to be the motor timing. We assume the current waveform is in phase with the magnetic field, basically. So we need to know the relative position of the two waveforms, as a function of total wave period/wavelength.
The conventional approach is to use atan on eta[0] and eta[1], which represent the fluxes between 2 of the phases, and also a clarke tranform on the current waveform from the current flowing through the terminals. Take the difference, that’s your motor timing. Use a PI control loop to regulate something, in this case I was planning on regulating voltage, actually, not speed, to keep motor timing in a good range. Obviously field oriented control is supposed to imply that this motor timing will be regulate to essentially 90 degrees, the optimal point for best torque and efficiency, but in reality I have found that that is not very common esp in sensorless drivers because there is too much noise and the system would be too prone to stalling if you did that. Once a sensorless system stalls you’d need some fancy thing like High frequency injection or something to get out of the stall, so motor timing usually just has some padding in there to keep things on the other side of the optimal point, at the cost of efficiency.
The code below gives the pd_mcs_float figure, which is supposed to reflect the time between the zero crossing points of the current waveform and the flux observer waveform. Dividing by total waveform period time should give a useful motor timing proxy to regulate. It runs at 15 kHz.
But it’s too noisy even after considerably filtering. I don’t really know why. Debugging is very hard because it moves pretty fast.
If I run the fan and turn the voltage on the power supply down, the figure goes up, which is what you would expect if it reflected the motor timing, because lower drive voltage. I can also look at the timestamps and they are happening essentially correctly. The number of microseconds is also too small, it should be larger at the point of stalling than it is.
I think the current is probably too noisy for this approach :(. I may have to wait till I can get the qvadrans and it’s better inline current sensing.
I’m interested in paying someone a bit if they may be able to help get this working…
#include <SimpleFOC.h>
#include <math.h>
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
//#define POLEPAIRS 7
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.05);
LowPassFilter pd_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;
float pd_s =0;
float pd_mcs_float = 0;
unsigned long int mcs_phase_diff = 0;
float W = 0;
float Q = 0;
float E = 0;
float R = 0;
float T = 0;
unsigned long int pd_mcs = 0;
float period_time = 0;
float c_zero_crossing_ts_mcs = 0;
float e_zero_crossing_ts_mcs = 0;
unsigned long int current_ts_fe;
unsigned long int last_ts_fe;
unsigned long int c_ts = 0;
unsigned long int e_ts = 0;
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 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;
}
}
}
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_average_c(float val){
#define NUM_SAMPLES 6
static float samples[NUM_SAMPLES];
static int sample_index = 0;
samples[sample_index] = val;
sample_index++;
if (sample_index >= NUM_SAMPLES) {
sample_index = 0;
}
float sum = 0;
for (int i = 0; i < NUM_SAMPLES; i++) {
sum += samples[i];
}
float average = sum / NUM_SAMPLES;
return average;
}
float track_average_mt(float val){
#define NUM_SAMPLES 10
static float samples[NUM_SAMPLES];
static int sample_index = 0;
samples[sample_index] = val;
sample_index++;
if (sample_index >= NUM_SAMPLES) {
sample_index = 0;
}
float sum = 0;
for (int i = 0; i < NUM_SAMPLES; i++) {
sum += samples[i];
}
float average = sum / NUM_SAMPLES;
return average;
}
float zero_cross_detection(float val){ //this is for the current, what is val in? amps? seems to be some other measure, should be calibrated and zeroed to be amps but no time now
static int state = 0;
static float output = 3;
static float sample_state1 = 0;
static unsigned long int ts_state1 = 0;
static float sample_state2 = 0;
static unsigned long int ts_state2 = 0;
float slope_at_zero = 0;
//the levels have to be in the right places, if the threshold for state 1 is too clos eto state 0 then it may flip on the upwards due to noise, if the threshold of state 1 is lower than 0 one of course it would flip immediately
if (state == 0){
if (val > 0.12){
state = 1;
}
}
if (state == 1){
if (val < 0.1){//if it's in state 1 and bets below
sample_state1 = val;
ts_state1 = micros();
state = 2;
}
}
if (state ==2){
if (val < -0.1){
sample_state2 = val;
ts_state2 = micros();
slope_at_zero = (sample_state2-sample_state1)/float(ticks_diff(ts_state2,ts_state1));
c_zero_crossing_ts_mcs = float(ts_state1)-(sample_state1/slope_at_zero);
pd_mcs_float = c_zero_crossing_ts_mcs-e_zero_crossing_ts_mcs;
state = 0;
return 1;//triggered!
}
}
return 0;
}
float zero_cross_detection2(float val){ //this one is for eta*100 (*100 to bring it into a visible range on the debugger) it goes from zero to 0.2 for this motor.
static int state = 0;
static float output = 3;
static float sample_state1 = 0;
static unsigned long int ts_state1 = 0;
static float sample_state2 = 0;
static unsigned long int ts_state2 = 0;
float slope_at_zero = 0;
if (state == 0){
if (val > 0.12){
state = 1;
}
}
if (state == 1){
if (val < 0.05){//if it's in state 1 and crosses down
sample_state1 = val;
ts_state1 = micros();
state = 2;
}
}
if (state == 2){
if (val < -0.05){
sample_state2 = val;
ts_state2 = micros();
slope_at_zero = (sample_state2-sample_state1)/float(ticks_diff(ts_state2,ts_state1));
e_zero_crossing_ts_mcs = float(ts_state1)-(sample_state1/slope_at_zero);
state = 0;
return 1;//triggered!
}
}
return 0;
}
float track_return_eta0(float v_pA, float v_pB, float current_A, float current_B, float current_C, int outputorno) {
// 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 float I_last_alpha_beta[] = {0.0f,0.0f};
static float last_etas[]= {0.0f,0.0f};
static float flux_state_[] = {0.0f,0.0f};
static float last_v_alpha = 0.0f;
static float last_v_beta = 0.0f;
static float last_phase = 0.0f;
static float observer_gain = 750.0f; //1500 seems to be about right, 3000 is too high? not clear actually. phase becomes nan somewhere between 10,000 and 15,000 could still be way too low. 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, terminal to terminal divided by two: 2.9
static float O_phase_inductance = 0.0006455f; //0.001291 old, actual terminal to terminal measurement divided by two: 0.0006455 in henries *presumably
static float O_flux_linkage = 0.0017f;// 0.0017 seems about right when resistance is 2.9 and inductance is 0.0006455, gives the smoothest line it's in units of webers *presumably, I measured it across the terminals and calculated it with the python program. let's try half what it was (was 0.00931).:
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()<40){ //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
float 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 = 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 / 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;
last_run_ts = micros();
last_etas[0] = eta[0];
last_etas[1] = eta[1];
I_last_alpha_beta[0] = I_alpha_beta[0];
I_last_alpha_beta[1] = I_alpha_beta[1];
float phase = atan2(eta[1], eta[0]); // do we have fast_atan available? no
last_phase = phase;
//test_variable = phase;
float stator_field_angle = atan2(I_alpha_beta[0],I_alpha_beta[1]); // these might be the wrong way around fuck. This is the most likely.
//float motor_timing = handle_wraparound_angle(phase, stator_field_angle, return_sign(motor.target));
//if (outputorno == 0){
return eta[0];
//}
};
//
//float handle_wraparound_angle(float angle_behind, float angle_ahead, int dir){
// static float piepie = 6.2831853f;
// if (dir == 1){ //clockwise drive
// if (angle_behind < angle_ahead){ // they are in the same cycle
// return (angle_ahead-angle_behind);
// }
// if (angle_behind > angle_ahead){ // the angle_ahead has gone past the end and wrapped around
// return (piepie-(angle_behind-angle_ahead));
// }
// }
// if (dir == 0){ //counter-clockwise drive
// if (angle_behind > angle_ahead){ // they are in the same cycle
// return (angle_ahead-angle_behind);
// }
// if (angle_behind < angle_ahead){ // the angle_ahead has gone past the end and wrapped around
// return ((-1*piepie)-(angle_behind-angle_ahead));
// }
// }
//}
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);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial2");
// 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-(driver.voltage_power_supply/2);
Vb = driver.dc_b * driver.voltage_power_supply-(driver.voltage_power_supply/2);
Q = track_average_c(A);
W = track_return_eta0(Va, Vb, A, B, C, 0)*100;
E = zero_cross_detection(Q);
R = zero_cross_detection2(W);
if (E == 1){ //current has crossed zero, falling.
c_ts = micros();
}
if (R == 1){ //eta has crossed zero, falling.
e_ts = micros();
}
return W;
}
int return_sign(float number){
if (number > 0){
return 1;
}
if (number <= 0){
return 0;
}
}
void loop() {
for (int i=0;i<5;i++){
static unsigned long int loop_clock_in = micros();
unsigned long int loop_time = 0;
float loop_time_s = 0;
loop_time = ticks_diff(micros(), loop_clock_in);
loop_clock_in=micros();
loop_time_s = float(loop_time)/1000000;
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;
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS)));
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel*loop_time_s;
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS)));
}
}
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)){
prop_V = (v_diff+fabs((motor.target*v_per_radsPS))); //constant run
}
}
if (motor.target > goal_speed + (accel*loop_time_s*1.5)){ //rps too positive, the (accel*loop_time_s etc stuff is to give a hysterises
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*loop_time_s*0.7;
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS)));
}
if (motor.target <= 0){
motor.target = motor.target-accel*loop_time_s; //counterclockwise rotation, accelerating
prop_V = (v_diff+accel_v_boost+fabs((motor.target*v_per_radsPS)));
}
}
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;
}
motor.move();
run_observer();
}
SerialComm();
// run_observer_with_output();
//Serial.print("c:");
//Serial.println(test_variable2);
// Serial.print("c:");
//Serial.println(track_average(Q));
// Serial.print(" ,t:");
// if (current_ts_fe > 1){
// Serial.println("3");
// }
// if (current_ts_fe <1 ){
// Serial.println("0");
// }
//Serial.println(current_ts_fe);
//period_time = 1/((motor.target*7)/6.2831853);
//pd_s = pd_mcs_float/1000000;//need a ticks diff for floats or this will ahve a problem when ticking over if it ever did might be impractically long but not good practice
if (pd_mcs_float<1000){
motortiming = pd_filter(pd_mcs_float);
}
//T = motortiming*100;
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);// this updates the current for the get dc current, that function won't work unless this is called each loop
overcurrent_trip();
}
There is a lot of short term noise, but the observer phase also drifts around on the several seconds time scale. I think it’s probably because when it gets run it lands sometimes on different rotor positions. A sort of beat frequency develops. In the end, it’s too crude. The odrive also could not drive this motor though, apparently. The observer can’t handle salience not even a little bit. I was hoping the waveform may be distorted but it would still be in phase. Looks like the phase drifts around too…
