I’ll just drop this off here for the next person who might be trying to implement a sensorless system. It doesn’t warrant it’s own github.
The key is the function that according to an equation I found somewhere, tried to dig up the article again but can’t find it, can give an approximation of the rotor position given the phase currents and the motor inductance. The inductance of my motor across the terminals is 1.3 mH, so that is why the value is the way it is. Obviously the code is severely underdeveloped. The figure it prints does go up when the difference between electrical and rotor angle goes up, and the point at which the motor stalls is reasonably consistent although it does vary from 0.8 to 0.95 or so along the rpm range from 50 rps to 250 rps, and it kind of flakes out due to some wraparound or something soon before the stall is reached at 250 rps. (rads per second).
With some corrections I think this type of approach could work, just a basic current observer. I wasn’t able to get the goods on a more sophisticated observer of any kind, luenberger observers seem to be the next most common type.
I think the above distortion might be due to my use of an smpmsm, not an ipmsm, which have different back emf and thus current waveforms.
It would be good to get a better system worked out, sensorless has a reputation for being for cheap stuff but actually I think that because the sensor and motor are the same unit it helps a lot to get rid of some complications. Let’s face it, magnetic angle sensors get complicated too.
#include <SimpleFOC.h>
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.05);
float goal_speed =0;
float v=2;
float v_diff=1;
int mode = 0;
float angle_for_angle_mode = 0;
float accel = 0.6;
float v_per_radsPS = 0.03;
float accel_v_boost = 0.5;
bool voltage_override = 1;
float A, B, C, X, Y, Z;
float theta = 0;
float lag = 0;
float e = 0;
float co, si, Dp,Qp;
float current_angle = 0;
float error=0;
float p_term = 0;
float p_gain = 0.0005;
float i_gain = 0.000005;
float i_term = 1;//start the voltage at 1 volt when pi controller starts up
float i_limit = 1;
float control_sig_limit = 1.5;
float control_signal = 1;
float setpoint = 0.7;
int dir;
float pid_input =0;
float L = 0.0013;
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); break;
case 'y': Serial.read(); Serial.print("v"); 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 'q': Serial.read(); Serial.print("q:"); Serial.println(p_gain); break;
case 'i': Serial.read(); Serial.print("i:"); Serial.println(i_gain); break;
case 'l': Serial.read(); Serial.print("L:"); Serial.println(L, 5); 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 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'T': break;
case 'C': break;
case 'V': break;
case 'P': break;
case 'B': break;
case 'O': break;
case 'S': break;
case 'A': break;
case 'M': break;
case 'W': ;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 'B': accel_v_boost = Serial.parseFloat(); break;
case 'O': voltage_override = Serial.parseFloat(); break;
case 'S': setpoint = Serial.parseFloat();break;
case 'D': diff_filter.Tf = Serial.parseFloat(); Serial.print("I");break;
case 'A': angle_for_angle_mode = Serial.parseFloat(); break;
case 'M': Serial.print("Mode_changed"); mode = int(Serial.parseFloat()); break;
case 'W': driver.voltage_power_supply = Serial.parseFloat();break;
case 'Q': p_gain = Serial.parseFloat(); Serial.print("P");break;
case 'I': i_gain = Serial.parseFloat(); Serial.print("I");break;
case 'L': L = Serial.parseFloat(); Serial.print("I");break;
//while(Serial.read() >= 0) ; //remove any extra stuff in the buffer after a single command was recieved, in case there are multiple commands in there.
}
}
}
float angle_from_currents(float u, float v, float w){
float alpha =0;
float beta =0 ;
float angle1 = 0;
alpha = u - v*0.5-w*0.5;
beta = v*0.866025-w*0.8660253;
angle1 = atan2((alpha-(L*u)), (beta-(L*v)));
if (angle1 < 0){
angle1 += _2PI;
}
return _2PI-(angle1);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial4");
// 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 = 3; // [V]
motor.velocity_limit = 320; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
// init motor hardware
motor.init();
motor.voltage_limit = 2;
goal_speed = 2;
}
void pi_controller(){// the input should be the present filtered phase angle difference, negative is counterclockwise positive is clockwise rotation, the shaft will lag the electrical
static unsigned long last_invoke_ts = micros();
unsigned long diff = 0;
float control_signal_pre =0;
static int pi_polarity = 1;
diff = ticks_diff(micros(),last_invoke_ts);
diff = (float(diff)/1000000);
error = pid_input-setpoint; // when input is more than the setpoint, the shaft during clockwise (positive) rotation lagging too much, with this equation the error will be negative so if gain is positive and control signal is added to the electrical rpm, the rpm of the electrical will go down, and the shaft can catch up. To reverse rotation I think both gain and setpoint need to be reversed.
p_term = error*p_gain*diff;
i_term = i_term+error*i_gain*diff;
if (i_term > (i_limit+0.01)){
i_term = i_limit;
}
if (i_term < -1*(i_limit+0.01)){
i_term = -1*i_limit;
}
control_signal_pre = (p_term + i_term)*pi_polarity;// + d_term;
if (control_signal_pre > control_sig_limit){
control_signal = control_sig_limit;
return;
}
if (control_signal_pre < -1*control_sig_limit){
control_signal = -1*control_sig_limit;
return;
}
control_signal = control_signal_pre;
last_invoke_ts = micros();
return;
}
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 (t1-t2);
}
float angle_diff(float a1,float a2, int dir){ // dir =1 is clockwise, -1 is counterclockwise a1 comes first in time, a2 after, or a2 is shaft and a1 is electrical. because the current angle goes at twice the rate, we have a problem with wraparound. You need to know the direction.
static float last_val = 0;
// wraparound may have occurred
if (dir==1){ //clockwise rotation, forward rotation, positive angle normally
if ((a2-a1)>0){ // normal situation
last_val = (a2-a1);
return (a2-a1); //then rotation is clockwise, so positive
}
if ((a2-a1)<0){ // seems to be backwards, assume it wrapped around or it's noise
if (abs(a2-a1)>0.3){ //if the aboslute value is large assume it wrapped around
last_val = _2PI-abs((a2-a1));
return (_2PI-abs((a2-a1)));
}
if (abs(a2-a1)<0.2){ //if the absolute value is small assume it's noise
return (last_val);
}
}
}
if (dir==-1){ //clockwise rotation, forward rotation, positive angle normally
if ((a2-a1)>0){ //wrapped around
return (-1*((_2PI)-(a2-a1))); //
}
if ((a2-a1)<0){ // as expected
return (a2-a1);
}
}
}
void loop() {
for (int q = 0; q<10;q++){
for(int j=0;j<5;j++){
if (motor.target < goal_speed-(accel*1.5)){//commutation speed not positive enough
if (motor.target < 0){//counterclockwise rotation, deaccelerating
motor.target = motor.target+accel*0.7;
motor.move();
motor.voltage_limit = (v_diff+accel_v_boost+(pow(abs(motor.target),1.03)*v_per_radsPS))*voltage_override;
}
if (motor.target >= 0){ //clockwise rotation, accelerating
motor.target = motor.target+accel;
motor.move();
motor.voltage_limit = (v_diff+accel_v_boost+(pow(abs(motor.target),1.03)*v_per_radsPS))*voltage_override;
}
}
if (motor.target>=goal_speed-(accel*1.5)){//steady run phase
if (motor.target<=goal_speed+(accel*1.5)){
motor.move();
motor.voltage_limit = (v_diff+(pow(abs(motor.target),1.03)*v_per_radsPS))*voltage_override; //constant run
}
}
if (motor.target > goal_speed + (accel*1.5)){ //commutation speed too positive
if (motor.target > 0){ //clockwise rotation, deaccelerating
motor.target = motor.target-accel*0.7;
motor.move();
motor.voltage_limit = (v_diff+accel_v_boost+(pow(abs(motor.target),1.03)*v_per_radsPS))*voltage_override;
}
if (motor.target <= 0){
motor.target = motor.target-accel; //counterclockwise rotation, accelerating
motor.move();
motor.voltage_limit = (v_diff+accel_v_boost+(pow(abs(motor.target),1.03)*v_per_radsPS))*voltage_override;
}
}
for (int i=0;i<5;i++){ // shouldloop at about 37 khz on b-g431 board, appears to be only 5khz on lepton
for (int q = 0; q<10;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
if (motor.target > 0){
dir = 1;
}
if (motor.target < 0){
dir = -1;
}
e = fmod((7*motor.shaft_angle), _2PI);
PhaseCurrent_s current1 = currentSense.getPhaseCurrents();
A = current1.a;
B = current1.b;
C = current1.c;
current_angle = angle_from_currents(A,B,C);
lag = angle_diff(current_angle, e, dir);
lag = diff_filter(lag);
pid_input = lag;
pi_controller();
}
Serial.print("l:");
Serial.print(lag);
Serial.print(",");
// Serial.print("e1:");
// Serial.print(motor.shaft_angle);
// Serial.print(",");
Serial.print("c:");
Serial.print(control_signal);
Serial.print(",");
Serial.print("err:");
Serial.print(error);
Serial.print(",");
Serial.print("setpoint:");
Serial.print(setpoint);
Serial.print(",");
Serial.print("i:");
Serial.print(i_term);
Serial.print(",");
Serial.print("p:");
Serial.println(p_term);
SerialComm();
}
//Serial.println("t");
/*if (abs(goal_speed) > motor.velocity_limit){
if(goal_speed<0){
goal_speed = motor.velocity_limit*(-1);
}
if(goal_speed>0){
goal_speed = motor.velocity_limit;
}
}
*/
}
}