I’ll just share my code for anyone who might be interested, as we previously discussed sensor error correction.
Magnetic angle sensors are a relatively cheap and good option, but esp with 3d printed components, they really do seem to need error correction measures to be usable for FOC, as the angle between the rotor and the electrical angle has to be regulated rather tighter than typical error tends to be.
The code base for simpleFOC already includes some nice sensor calibration stuff, but having looked at it, I think it tends to bake in the cogging of the motor. It seems to go at low speed and assume the electrical angle is the mechanical angle, which is not quite right. Also I wasn’t clear if it compensates for normal lag between the electrical (magnetic?) angle and the rotor due to friction (I think it might as it does go both ways, same as the approach below, which is part of compensating for that).
This approach saves your calibration data to an ASCII file, a text tile basically though the file name ends in .json . So you can analyze it and save it for future use etc. instead of doing it every time.
It occurs to me that this still isn’t the best way, it still bakes in cogging to some degree. I think a better way would be to basically mathematically model the system to a significant degree and tease apart the different sources of error. This doc for instance is the best I could find on the nature of the errors, and as you can see the waveforms of most error sources could probably be identified and eliminated more directly by using an equation that mimicked the physics involved.
That would be way more work, though, so as usual if we are all divided all anyone can really do is deal with their own problem as quickly as they can and best case scenario share the code… This solution works fine for me because I am using low cogging motors anyway and I don’t need precise motion or anything, it’s just a fan. I tried to do it without any kind of error correction though and that wasn’t viable.
This approach does the process at a higher angular velocity and you could attach a mass to the rotor for better accuracy still. You can also adjust the number of sample points etc.
It’s pretty crudely written of course, but it works ok and is fast enough.
The micropython runs on the raspberry pi pico, but it shouldn’t be a problem to use an ESP32 or other micropython board with minimal changes. The arduino code runs on a B-G431B-ESC1 board but with minimal adaptation for pinouts etc. except you also would need to adjust it for the voltage/rpm etc for your motor, also it’s probably not useful for most other projects to actually drive the motor during use, a fan is rather uniquely easy to drive as the load is very smooth and predictable. In particular the responsiveness of the device to serial inputs is slow because I prioritized minimal checking of the serial port so it doesn’t check for data more than about once every 70 msec IIRC. There is also some extra stuff I left in there like angle mode and the ticks_diff function but I don’t want to change it as I would have to test it again before posting it if I did that. The extra move commands actually do help to smooth the signal even if they look messy.
#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);
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;
void SerialComm()
{
if (Serial.available() > 0){
switch(Serial.read())
{
case 'T': goal_speed = Serial.parseFloat();break;
case 't': Serial.print("t"); Serial.println(goal_speed); break;
case 'C': accel = Serial.parseFloat();break;
case 'c': Serial.print("c"); Serial.println(accel); break;
case 'V': v_diff = Serial.parseFloat(); break;
case 'v': Serial.print("v"); Serial.println(motor.voltage_limit); break;
case 'P': v_per_radsPS = Serial.parseFloat(); break;
case 'p': Serial.print("p"); Serial.println(v_per_radsPS, 4); break;
case 'B': accel_v_boost = Serial.parseFloat(); break;
case 'b': Serial.print("b"); Serial.println(accel_v_boost); break;
case 'O': voltage_override = Serial.parseFloat(); break;
case 'o': Serial.print("o"); Serial.println(voltage_override); break;
case 'S': motor.target = Serial.parseFloat();break;
case 's': Serial.print("s"); Serial.println(motor.target); break;
case 'e': 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 '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 'w': Serial.print("w"); Serial.println(driver.voltage_power_supply); 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.
}
}
}
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);
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;
}
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));
}
}
void loop() {
switch(mode)
{
case 0:
motor.controller = MotionControlType::velocity_openloop;
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<50;i++){ // shouldloop at about 37 khz on b-g431 board, appears to be only 5khz on lepton
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
}
//Serial.println("t");
SerialComm();
/*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;
}
}
*/
}
break;
case 1:
motor.controller = MotionControlType::angle_openloop;
for (int w = 0 ; w < 10; w++){
SerialComm();
motor.move(angle_for_angle_mode);
motor.voltage_limit = v;
}
break;
}
}
Code to collect calibration data. First it measures the electrical lag, the lag between the true mechanical angle and the electrical angle, under certain conditions of rps and voltage. It makes a list of lists to represent intervals somewhere within which samples are to be taken, then it grabs samples that fit in the intervals and builds a list of lists which relates the sensor angle to the assumed true mechanical angle:
from machine import ADC, Pin
from time import ticks_ms, ticks_diff, ticks_us, sleep
from filter2 import *
from phase_diff_sensor_calib_filter import *
from velocity_filter import *
from machine import UART
from math import fmod, sin, cos
import json
import sys
uart1=UART(1, 115200)
uart1.init(1000_000, tx=Pin(4),rx=Pin(5),timeout_char=1)# the timeout should be reduced after things appear to be working, to increase responsiveness of the e read
commanded_direction_of_rotation = 1
pi=3.1415926535897932384626433832795
goal_T=2# in rads per second
actual_cfm_goal = 2
voltage = 1
v_diff = 1
wind_correction_figure = 0
wind_correction_scaling = 0.5
pole_pairs = 7
v_check_clock_in = ticks_us()
v_check_last_rads = 0
led=Pin(25, Pin.OUT)
setpoint = 2
p_gain = 0.2
i_gain = 0.2
i_limit = 1
control_signal = v_diff
i_term = 0
pi_clock_in = 0
uart0=UART(0, 115200)
uart0.init(1000000, tx=Pin(0),rx=Pin(1),timeout_char=1)
uart0.write("test")
def SerialComm_uart0():
global goal_T
global v_diff
global actual_cfm_goal
global setpoint
global i_gain
global p_gain
try:
if uart0.any()>0:
message=uart0.read()
string = str(message)
string = string[2:-3]# there is extra stuff that gets added during transmission, chop it off.
command_type=string[0]
if command_type == "T":#The pico W is telling you to accelerate or deaccelerate to a new rads per second
goal_T=float(string[1:])
if command_type == "t": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(goal_T)+"\n"
uart0.write(t_reply)
if command_type == "c": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(actual_cfm_goal)+"\n"
uart0.write(t_reply)
if command_type == "v": # the pico W is asking what's the actual voltage you last sent to the wavegen
t_reply = str(v_diff)+"\n"
uart0.write(t_reply)
if command_type == "p": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(p_gain)+"\n"
uart0.write(t_reply)
if command_type == "i": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(i_gain)+"\n"
uart0.write(t_reply)
if command_type == "s": # the pico W is asking what's the actual voltage you last sent to the wavegen
t_reply = str(setpoint)+"\n"
uart0.write(t_reply)
if command_type == "V":
v_diff=float(string[1:])
if command_type == "P":
p_gain=float(string[1:])
if command_type == "I":
i_gain=float(string[1:])
if command_type == "S":
setpoint=float(string[1:])
if command_type == "C":
actual_cfm_goal=float(string[1:])
except:
print("error, some serial command probably borked, ignore it and carry on")
def init_wavegen(v_diff=1,accel=0.6,v_per_radsPS = 0.03,accel_v_boost = 0.5): #the wavegen already has default values, you only need to call this if you want to change them.
set_v_diff(v_diff)
sleep (0.1)
command = "C"+str(accel)+"\n"
uart1.write(command)
sleep (0.1)
command = "P"+str(v_per_radsPS)+"\n"
uart1.write(command)
sleep (0.1)
command = "B"+str(accel_v_boost)+"\n"
uart1.write(command)
angle_pin = ADC(Pin(27))
angle_min = 0
angle_max = 65535
def get_rads():
global angle_min
global angle_max
global pi
rads = (angle_pin.read_u16()-angle_min)/(angle_max-angle_min) *2*3.141592653
return rads
def check_e():
start_time = ticks_us()
tries = 0
stuff_ready = 0
uart1.write("e")
while ticks_diff(ticks_us(),start_time)<50000:#wait for a max of 70 msec for reply
if uart1.any()>=1:
break
message=uart1.read()
string = str(message)
string = string[3:-5] # there is extra stuff that gets added during transmission, plus the e, chop it off.
return float(string) # this should be a floating point number with 3 digits after the decimal point, varying from zero to 2pi
def set_v_diff(v_diff_to_send):
command = "V"+str(v_diff_to_send)+"\n"
uart1.write(command)
def set_rps(rps):#rps is rads per second
command = "T"+str(rps)+"\n"
uart1.write(command)
def angle_diff_2(a2,a1): #a1 should be shaft or earlier in time, a2 should be electrical or later in time. We are assuming the actual angle is less than pi because if it was more it would skip a step and restablize at a new angle or stall or something
global pi
if abs(a2-a1)>pi:#if either has wrapped around
if a2<a1:# clockwise rotation
return (2*pi-abs(a1-a2))
if a2>a1:#must be counterclockwise rotation
return -1*(2*pi-abs(a1-a2))
else:
return a2-a1
def check_lag():#the lag will change depending on rps and voltages etc so we just measure it under given conditions then use the same conditions for sensor calibration
sleep(0.1)
set_v_diff(1)
sleep(0.1)
set_rps(10)
sleep(0.1)
#measure forward phase diff
start_time = ticks_ms()
while ticks_diff(ticks_ms(),start_time)<40_000:
elec_angle = fmod(abs(check_e())*7,2*pi)
raw_sensor = fmod(get_rads()*7,2*pi)
angle_dif = angle_diff_2(elec_angle,raw_sensor) # the subtracted figure is just the sensor offset I measured manually, have a zeroing routine for the future.
diff_f = sen_cal_fil_pd(angle_dif)
print("diff_f:", diff_f/7,"elec_angle:",elec_angle,"raw_sensor:",raw_sensor)
sleep(0.1)
print("diff_f:", diff_f/7)
reset_sen_cal_fil()
print("filter reset:", sen_cal_fil_pd(0))
print("reversing direction")
sleep(0.1)#have to wait, if you communicate with the wavegen too frequently it doesn't answer
set_rps(-10)
sleep(2)#let it accelerate
start_time = ticks_ms()
while ticks_diff(ticks_ms(),start_time)<40_000:
# elec_angle = fmod(((2*pi)-abs(check_e()))*7,2*pi)
elec_angle = fmod(abs(check_e())*7,2*pi)
raw_sensor = fmod(get_rads()*7,2*pi)
angle_dif = angle_diff_2(elec_angle,raw_sensor) # the subtracted figure is just the sensor offset I measured manually, have a zeroing routine for the future.
diff_r = sen_cal_fil_pd(angle_dif)
print("diff_r:", diff_r/7,"elec_angle:",elec_angle,"raw_sensor:",raw_sensor)
sleep(0.1)
print("diff_r:", diff_r/7)
lag = (abs(diff_r/7)+abs(diff_f/7))/2
return lag
#print("sensor offset, this will change depending on the rotor start position and is not used:", ((abs(diff_r)+abs(diff_f))/2)-diff_f)
def fmod2(num,divisor):# the built in fmod doesn't handle negative numbers the way I want, or the way the documentation says it should
if num >=0:
return fmod(num, divisor)
if num < 0:
borked_value = fmod(num, divisor)
rightval = divisor + borked_value
return rightval
def elec_to_mech(elec,lag,direction):#1 is clockwise, -1 is counterclockwise
global pi
if direction == 1:
mech = elec-lag
out = fmod2(mech,2*pi)
if direction == -1:
mech = elec+lag
out = fmod2(mech,2*pi)
return out
def ccw_angle_diff(a2,a1):#a2 comes after a1 in time or a1 is sensor and a2 is electrical during forward rotation. this is similar to a2-a1 but with wraparound
global pi
diff = a2-a1
if diff >=0:
ret = diff
if diff < 0:
ret = 2*pi+diff
return ret
def step_offset_determine(): # rotation has to be clockwise or e gets messed up e becomes negative, correct that deeper down later. This may not work with very large errors in the sensor
global pi
global pole_pairs
sleep(0.1)
elec_angle = check_e()
raw_sensor = get_rads()
diff = ccw_angle_diff(elec_angle,raw_sensor)
steps_offset = int(diff/(2*pi/pole_pairs))
print("steps offset:",steps_offset)
sleep(0.1)
return steps_offset # this is the figure you subtract from elec to correct for starting in random step positions
def corrected_elec(elec,step_offset):
global pole_pairs
global pi
elec = elec - (step_offset*2*pi/pole_pairs)
elec = fmod2(elec,2*pi)
return elec
lag = check_lag() #previously measured just takes a while to measure so I recorded it here
print("lag:", lag)
sys.exit()
sleep(0.1)
set_v_diff(1)
sleep(0.1)
set_rps(10)
sleep(2) #let it accelerate
step_offset = step_offset_determine()
intervals = []
window_width = 1/50#this should be shrunken after testing, but it takes longer to collect samples when it's smaller. make sure the width is always smaller than the interval
num_windows = 100# we actually omit some windows so it's not this many
upper_limit= int((2*pi-window_width-0.03)*1_000_000)#range function only works with integers so we have to convert to int and then back again, also the sensor has a dead spot close to zero and 2pi radians so we need to remove that window
interval = int(2*pi*1_000_000/num_windows)
for i in range(50_000, upper_limit , interval):#the range doesn't start at zero to avoid the sensor dead spot
intervals.append([i/1_000_000,(i/1_000_000)+window_width])
print("intervals:", intervals)
samples = [None]*len(intervals)
x = 1
while x>0:
elec_angle = check_e()
elec_angle = corrected_elec(elec_angle,step_offset)
raw_sensor = get_rads()
assumed_mech = elec_to_mech(elec_angle,lag,1)
for i in range(len(intervals)):
if raw_sensor >= intervals[i][0] and raw_sensor <= intervals[i][1]:
samples[i]=[raw_sensor, assumed_mech]
print("sample fit in a window (sensor, mech):", [raw_sensor, assumed_mech])
x = 0
print(x)
for i in samples:
if i is None:
x=x+1
print("empty slots:", x)
print("done measuring samples:", samples)
with open("samples.json", "w") as outfile:
json.dump(samples, outfile)
This code uses the data:
from machine import ADC, Pin
from time import ticks_ms, ticks_diff, ticks_us, sleep
from filter2 import *
from phase_diff_sensor_calib_filter import *
from velocity_filter import *
from machine import UART
from math import fmod, sin, cos
import json
import sys
import gc
uart1=UART(1, 115200)
uart1.init(1000_000, tx=Pin(4),rx=Pin(5),timeout_char=1)# the timeout should be reduced after things appear to be working, to increase responsiveness of the e read
commanded_direction_of_rotation = 1
pi=3.1415926535897932384626433832795
goal_T=100# in rads per second
actual_cfm_goal = 2
voltage = 1
v_diff = 0
wind_correction_figure = 0
wind_correction_scaling = 0.5
pole_pairs = 7
v_check_clock_in = ticks_us()
v_check_last_rads = 0
led=Pin(25, Pin.OUT)
setpoint = 2
p_gain = 0.2
i_gain = 0.2
i_limit = 1
control_signal = v_diff
i_term = 0
pi_clock_in = 0
uart0=UART(0, 115200)
uart0.init(1000000, tx=Pin(0),rx=Pin(1),timeout_char=1)
uart0.write("test")
def SerialComm_uart0():
global goal_T
global v_diff
global actual_cfm_goal
global setpoint
global i_gain
global p_gain
try:
if uart0.any()>0:
message=uart0.read()
string = str(message)
string = string[2:-3]# there is extra stuff that gets added during transmission, chop it off.
command_type=string[0]
if command_type == "T":#The pico W is telling you to accelerate or deaccelerate to a new rads per second
goal_T=float(string[1:])
if command_type == "t": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(goal_T)+"\n"
uart0.write(t_reply)
if command_type == "c": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(actual_cfm_goal)+"\n"
uart0.write(t_reply)
if command_type == "v": # the pico W is asking what's the actual voltage you last sent to the wavegen
t_reply = str(v_diff)+"\n"
uart0.write(t_reply)
if command_type == "p": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(p_gain)+"\n"
uart0.write(t_reply)
if command_type == "i": # the pico W is asking what's the actual rpm you last sent to the wavegen
t_reply = str(i_gain)+"\n"
uart0.write(t_reply)
if command_type == "s": # the pico W is asking what's the actual voltage you last sent to the wavegen
t_reply = str(setpoint)+"\n"
uart0.write(t_reply)
if command_type == "V":
v_diff=float(string[1:])
if command_type == "P":
p_gain=float(string[1:])
if command_type == "I":
i_gain=float(string[1:])
if command_type == "S":
setpoint=float(string[1:])
if command_type == "C":
actual_cfm_goal=float(string[1:])
except:
print("error, some serial command probably borked, ignore it and carry on")
def init_wavegen(v_diff=1,accel=0.6,v_per_radsPS = 0.03,accel_v_boost = 0.5): #the wavegen already has default values, you only need to call this if you want to change them.
set_v_diff(v_diff)
sleep (0.1)
command = "C"+str(accel)+"\n"
uart1.write(command)
sleep (0.1)
command = "P"+str(v_per_radsPS)+"\n"
uart1.write(command)
sleep (0.1)
command = "B"+str(accel_v_boost)+"\n"
uart1.write(command)
angle_pin = ADC(Pin(27))
angle_min = 0
angle_max = 65535
def get_rads():
global angle_min
global angle_max
global pi
rads = (angle_pin.read_u16()-angle_min)/(angle_max-angle_min) *2*3.141592653
return rads
def check_e():
start_time = ticks_us()
tries = 0
stuff_ready = 0
uart1.write("e")
while ticks_diff(ticks_us(),start_time)<50000:#wait for a max of 70 msec for reply
if uart1.any()>=1:
break
message=uart1.read()
string = str(message)
string = string[3:-5] # there is extra stuff that gets added during transmission, plus the e, chop it off.
return float(string) # this should be a floating point number with 3 digits after the decimal point, varying from zero to 2pi
def set_v_diff(v_diff_to_send):
command = "V"+str(v_diff_to_send)+"\n"
uart1.write(command)
def set_rps(rps):#rps is rads per second
command = "T"+str(rps)+"\n"
uart1.write(command)
def angle_diff_2(a2,a1): #a1 should be shaft or earlier in time, a2 should be electrical or later in time. We are assuming the actual angle is less than pi because if it was more it would skip a step and restablize at a new angle or stall or something
global pi
if abs(a2-a1)>pi:#if either has wrapped around
if a2<a1:# clockwise rotation
return (2*pi-abs(a1-a2))
if a2>a1:#must be counterclockwise rotation
return -1*(2*pi-abs(a1-a2))
else:
return a2-a1
def load_samples():
with open("samples.json","r") as openfile:
samples = json.load(openfile)
return samples
def sensor_to_true(sensor_angle):#this is all assuming clockwise for now.
global samples
global pi
s_a_beginning = None
len_samples=(len(samples))
for i in range(len(samples)):
sample_int_length = angle_diff_2(samples[((i+1)%len_samples)][0], samples[i][0])
if sensor_angle >= samples[i][0] and sensor_angle <=samples[i][0]+sample_int_length:
s_a_beginning = samples[i][0]
s_a_end = samples[((i+1)%len_samples)][0]
mech_beginning = samples[i][1]
mech_end = samples[((i+1)%len_samples)][1]
if s_a_beginning == None:
print("error, sensor angle input doesn't fall into any interval measured, probably fell in dead zone at the top")
return 0
distance = angle_diff_2(s_a_end, s_a_beginning)
distance_past_s_a_beginning = angle_diff_2(sensor_angle,s_a_beginning)
relative_distance_along_interval = distance_past_s_a_beginning/distance
mech_distance = angle_diff_2(mech_end, mech_beginning)
dist_from_mech_beg = mech_distance*relative_distance_along_interval
mech_angle = mech_beginning+dist_from_mech_beg
mech_angle = fmod(mech_angle, 2*pi)
return mech_angle
def get_cor_sen_reading():
raw_reading = get_rads()
reading = sensor_to_true(raw_reading)
return reading
def ccw_angle_diff(a2,a1):#a2 comes after a1 in time or a1 is sensor and a2 is electrical during forward rotation. this is similar to a2-a1 but with wraparound
global pi
diff = a2-a1
if diff >=0:
ret = diff
if diff < 0:
ret = 2*pi+diff
return ret
def step_offset_determine(): # This may not work with very large errors in the sensor, and should probably be done under the same direction of rotation etc as sensor calibration was(clockwise, usually)
global pi#the motor can be in any of x different positions, where x is the number of pole pairs. We need to know which sector it's in when it first powers on basically
global pole_pairs#everything would get messed up if the motor was stuck
#sleep(0.06)# you need to make sure you haven't communicated with the wavegen for about 50 milliseconds before asking for e
elec_angle = check_e()
sensor_reading = get_cor_sen_reading()
diff = ccw_angle_diff(elec_angle,sensor_reading)
steps_offset = int(diff/(2*pi/pole_pairs))
return steps_offset # this is the figure you subtract from elec to correct for starting in random step positions, it should put the e in the same sector as the sensor value
def corrected_elec(elec,step_offset):
global pole_pairs
global pi
elec = elec - (step_offset*2*pi/pole_pairs)
elec = fmod2(elec,2*pi)
return elec
def fmod2(num,divisor):# the built in fmod doesn't handle negative numbers the way I want, or the way the documentation says it should
if num >=0:
return fmod(num, divisor)
if num < 0:
borked_value = fmod(num, divisor)
rightval = divisor + borked_value
return rightval
#initialization
init_wavegen()
sleep(0.1)
set_rps(100)
samples = load_samples()
#print("samples:", samples)
sleep(0.1)
step_offset = step_offset_determine()
while True:#main loop
for i in range(0,20):
sleep(0.1)
elec_angle = check_e()
sensor_reading = get_rads()
elec_angle = corrected_elec(elec_angle,step_offset) #this won't work for counterclockwise, e reads negative go correct itin the wavegen.
true_angle = sensor_to_true(sensor_reading)
print( "true_angle:", true_angle,"elec_angle:", elec_angle, "sensor reading:",sensor_reading, "diffx7:",7*angle_diff_2(elec_angle,true_angle))
SerialComm_uart0()
sleep(0.05)
set_rps(goal_T)
sleep(0.05)
set_v_diff(v_diff)
You would also need these files come to think of it, obviously these filters should be implemented as a class and then you do things rather differently but I haven’t gotten around to that yet:
filter2.py:
from time import ticks_us, ticks_diff
filter1_last_clock = 0
filter1_y_prev = 0
filter1_time_constant = 2
def lowpass_filter(sample):
global filter1_last_clock
global filter1_time_constant
global filter1_y_prev
sample1=sample
#print (sample1)
clock_now = ticks_us()
dt = ticks_diff(clock_now, filter1_last_clock)/1000000
alpha = filter1_time_constant/(filter1_time_constant+dt)
y = (alpha*filter1_y_prev)+((1-alpha)*sample1)
filter1_y_prev = y
filter1_last_clock = clock_now
#print (filter1_time_constant)
return y
phase_diff_sensor_calib_filter.py:
from time import ticks_us, ticks_diff
filter1_last_clock = 0
filter1_y_prev = 0
filter1_time_constant = 10
def sen_cal_fil_pd(sample):
global filter1_last_clock
global filter1_time_constant
global filter1_y_prev
sample1=sample
#print (sample1)
clock_now = ticks_us()
dt = ticks_diff(clock_now, filter1_last_clock)/1000000
alpha = filter1_time_constant/(filter1_time_constant+dt)
y = (alpha*filter1_y_prev)+((1-alpha)*sample1)
filter1_y_prev = y
filter1_last_clock = clock_now
#print (filter1_time_constant)
return y
def reset_sen_cal_fil():
global filter1_y_prev
filter1_y_prev = 0
I am using an AS5600 sensor with analog output (no need to reprogram it, it does this by default, but you have to remove resistor R4 on most boards or it doesn’t give an analog output). I added a 470 ohm resistor and 1uf capacitor to filter the signal however this is contributing to one source of glitch which is caused apparently when the sample of the angle is taken at an inopportune moment when the angle sensor is wrapping around from 2pi to 0 or vice versa, and thus the voltage is in between. The ADC samples the voltae and gets a value that is way off, however I am hoping a low pass filter will obviate that glitch.
Oh, there is one remaining issue too with the end points of the sensor sample list, right now if it’s before the first sample or after the last sample it just returns 0, however this would actually be an error of up to 0.06 rads, which would be significant but wouldn’t happen very often so it’s not really an issue for me.