Hm I did a quick experiment today to see if I could make this happen. It does work but it’s extremely slow, and it tends to overshoot and cause stall if I increase the P gain any more. The problem seems to be that the polarity detection keeps flipping direction. It needs a longer sampling period I think, the noise is too large right now.
#include <SimpleFOC.h>
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.01);
bool voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float p_gain = 0.1;// rads per second per amp
int controller_polarity = 1;
float stpt_adjust_rate = 0;//in amps per second
float setpoint_down_bias = 0.05; // in amps
float lowest_seen_current =1;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;
case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(p_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'q': Serial.read(); Serial.print("q"); Serial.println(stpt_adjust_rate); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y': break;
case 'U': break;
case 'O': break;
case 'F': break;
case 'Q': break;
case 'S': break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.
}
}
if (Serial.available() >= 9){
switch(Serial.read())
{
case 'T': goal_v = Serial.parseFloat();break;
case 'C': p_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':p_gain = Serial.parseFloat();break;
case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;
}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
// float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
// if (get_mA() > current_cap){
// voltage_override = 0;
// }
//}
void adjust_lsc_up(){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float adjustment = time_since_last_call_s*stpt_adjust_rate;
lowest_seen_current += adjustment;
}
unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return 1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float p_controller(float clf){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float error = clf-setpoint;
float p_term = p_gain*error*time_since_last_call_s*controller_polarity*(-1);
float new_rps = motor.target+p_term;
Serial.print("mA:");
Serial.print(get_mA());
Serial.print(",");
Serial.print("rps:");
Serial.print(new_rps/100);
Serial.print(",");
Serial.print("p:");
Serial.print(p_term*100);
Serial.print(",");
Serial.print("error:");
Serial.println(error);
return new_rps;
}
int detect_polarity(float clf, float rps){
static float last_rps = rps;
float rps_diff = rps-last_rps;
last_rps = rps;
static float last_clf=clf;
float clf_diff = clf-last_clf;
last_clf=clf;
if (rps_diff >= 0){
if (clf_diff >= 0){
return 1;
}
if (clf_diff < 0){
return -1;
}
}
if (rps_diff < 0){
if (clf_diff > 0){
return -1;
}
if (clf_diff <= 0){
return 1;
}
}
else{
return 1;//that shouldn't happen but just in case
}
}
void adjust_voltage(){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}
float adjust_setpoint_down(float clf){
if (clf<lowest_seen_current){
lowest_seen_current = clf;
}
return (lowest_seen_current-setpoint_down_bias);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2; // [V]
motor.velocity_limit = 320; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
setpoint = adjust_setpoint_down(get_mA());
Serial.println(setpoint);
Serial.println(get_mA());
}
}
//float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
// float x =0;
// x = currentlf_now*motor.voltage_limit/24;
// return 1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
//}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
// unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;
for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
setpoint = adjust_setpoint_down(get_mA());
}
adjust_voltage();
controller_polarity = detect_polarity(get_mA(), motor.target);
motor.target = p_controller(get_mA());
adjust_lsc_up();
// overcurrent_trip();
SerialComm();
}
This one stabilizes within a few seconds and optimizes properly but again quite slow, the next step would be to make all the variables that need to be tuned accessible over serial and see how responsive I could make it. It stalls if you reduce the voltage too fast, of course.
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.1);
bool voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float p_gain = 0.3;// rads per second per amp
int controller_polarity = 1;
float stpt_adjust_rate = 30;//in milliamps per second
float setpoint_down_bias = 2; // in milliiamps
float lowest_seen_current =1;
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;
case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(p_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
case 'q': Serial.read(); Serial.print("q"); Serial.println(stpt_adjust_rate); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
case 'j': Serial.read(); Serial.print("j"); Serial.println(setpoint_down_bias); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y': break;
case 'U': break;
case 'O': break;
case 'F': break;
case 'Q': break;
case 'S': break;
case 'J': break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.
}
}
if (Serial.available() >= 9){
switch(Serial.read())
{
case 'T': goal_v = Serial.parseFloat();break;
case 'C': p_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;
case 'J': setpoint_down_bias = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':p_gain = Serial.parseFloat();break;
case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;
}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
// float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
// if (get_mA() > current_cap){
// voltage_override = 0;
// }
//}
void adjust_lsc_up(){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float adjustment = time_since_last_call_s*stpt_adjust_rate;
lowest_seen_current += adjustment;
}
unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return 1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float p_controller(float clf){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
float error = clf-setpoint;
float p_term = p_gain*error*time_since_last_call_s*controller_polarity*(-1);
float new_rps = motor.target+p_term;
Serial.print("stp:");
Serial.print(setpoint);
Serial.print(",");
Serial.print("mA:");
Serial.print(get_mA());
Serial.print(",");
Serial.print("rps:");
Serial.print(new_rps);
Serial.print(",");
Serial.print("p:");
Serial.print(p_term*100);
Serial.print(",");
Serial.print("error:");
Serial.println(error);
return new_rps;
}
int detect_polarity(float clf, float rps){
static unsigned long int last_call_clock = micros();
static float last_rps = rps;
static float last_clf=clf;
static int val = 1;
static unsigned long int last_check = micros();
unsigned long int time_since_last_check = ticks_diff(micros(),last_check);
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
if ((time_since_last_check)>300000){
last_check = micros();
float rps_diff = rps-last_rps;
last_rps = rps;
float clf_diff = clf-last_clf;
last_clf=clf;
if (rps_diff >= 0){
if (clf_diff >= 0){
val=1;
}
if (clf_diff < 0){
val=-1;
}
}
if (rps_diff < 0){
if (clf_diff > 0){
val=-1;
}
if (clf_diff <= 0){
val= 1;
}
}
}
return val;
}
void adjust_voltage(){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}
float adjust_setpoint_down(float clf){
if (clf<lowest_seen_current){
lowest_seen_current = clf;
}
return (lowest_seen_current-setpoint_down_bias);
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2; // [V]
motor.velocity_limit = 320; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
}
setpoint = adjust_setpoint_down(get_mA());
// Serial.println(setpoint);
Serial.println(get_mA());
}
}
//float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
// float x =0;
// x = currentlf_now*motor.voltage_limit/24;
// return 1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
//}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
// unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;
for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
setpoint = adjust_setpoint_down(get_mA());
}
adjust_voltage();
controller_polarity = detect_polarity(get_mA(), motor.target);
motor.target = p_controller(get_mA());
adjust_lsc_up();
// overcurrent_trip();
SerialComm();
}
}
Also it wavers substantially on either side of the optimal point, but that’s sort of just a matter of tuning.
Hm, couldn’t get it to work very fast, even with tuning, but the variables seem to be responding plenty fast, it’s just a matter of the optimizer. IT only accelerates and only works cw, just a quick test of the theory.
Ok this one uses more of a rollin gball thing, using the slope instead of the setpoint adjustment of a P controller :
#include <SimpleFOC.h>
// NUMBER OF POLE PAIRS, NOT POLES, specific to the motor being used!
BLDCMotor motor = BLDCMotor(7);
//this line must be changed for each board
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
LowPassFilter diff_filter = LowPassFilter(0.05);
LowPassFilter slope_filter = LowPassFilter(0.5);
float voltage_override = 1;
float currentlf_now =0;
float current_limit_slope = 1.6;// this is in milliamps pre rad per second
float current_limit_o_term = 200;//this is the current limit at zero rps, it may not trip with stall
float maybe_o = 1;
float goal_v = 3;
float v_roc = 0.3;// this is voltage rate of change in volts per second
//float setpoint = 0;// this is adjusted to be the currentlf_now regularly
float d_gain = 0.3;// rads per second per amp
int controller_polarity = -1;
float perturb_rate = 3;//radians per second per second
float perturbation_amplitude = 1;//rads per second
//float stpt_adjust_rate = 30;//in milliamps per second
//float setpoint_down_bias = 2; // in milliiamps
//float lowest_seen_current =1;
long int perturb_cycle = 200000;// in microseconds.
void SerialComm(){
if (Serial.available() > 0){
switch(Serial.peek()){
case 't': Serial.read(); Serial.print("t"); Serial.println(goal_v); break;
case 'v': Serial.read(); Serial.print("v"); Serial.println(motor.voltage_limit); break;
case 'o': Serial.read(); Serial.print("o"); Serial.println(voltage_override); break;
case 's': Serial.read(); Serial.print("s"); Serial.println(motor.target); break;
case 'g': Serial.read(); Serial.print("g"); Serial.println(currentSense.getDCCurrent()); break;
case 'f': Serial.read(); Serial.print("f"); Serial.println(d_gain); break;
case 'w': Serial.read(); Serial.print("w"); Serial.println(driver.voltage_power_supply); break;
// case 'q': Serial.read(); Serial.print("q"); Serial.println(stpt_adjust_rate); break;
case 'y': Serial.read(); Serial.print("y"); Serial.println(current_limit_slope); break;
// case 'x': Serial.read(); Serial.print("x"); Serial.println(setpoint); break;
case 'u': Serial.read(); Serial.print("u"); Serial.println(current_limit_o_term); break;
// case 'j': Serial.read(); Serial.print("j"); Serial.println(setpoint_down_bias); break;
case 'e': Serial.read(); Serial.print("e"); if (motor.shaft_angle >= 0){
Serial.println(motor.shaft_angle, 3);
}
if (motor.shaft_angle < 0){
Serial.println((_2PI-(-1*motor.shaft_angle)), 3);
}
break;
case 'T': break;
case 'C': break;
case 'Y': break;
case 'U': break;
case 'O': break;
case 'F': break;
case 'Q': break;
case 'S': break;
case 'J': break;
default: Serial.read(); break; //if anything we don't recognize got in the buffer, clear it out or it will mess things up.
}
}
if (Serial.available() >= 9){
switch(Serial.read())
{
case 'T': goal_v = Serial.parseFloat();break;
case 'C': d_gain = Serial.parseFloat(); break;
case 'Y': current_limit_slope = Serial.parseFloat(); break;
case 'U': current_limit_o_term = Serial.parseFloat(); break;
// case 'J': setpoint_down_bias = Serial.parseFloat(); break;
case 'O':
maybe_o = Serial.parseFloat(); // just in case the wrong data gets in somehow we don't want the voltage going crazy
if (maybe_o < 1){
voltage_override = 0;
}
if (maybe_o >= 0.999){
voltage_override = 1;
}
break;// if it's not one of these, ignore it.
case 'F':d_gain = Serial.parseFloat();break;
// case 'Q': stpt_adjust_rate = Serial.parseFloat(); break;
case 'S': motor.target = Serial.parseFloat(); break;
}
}
}
//void overcurrent_trip(){// if it stalls this won't help except at higher powers, probably. Just helps prevent disaster
// float current_cap = current_limit_o_term + fabs(motor.target)*current_limit_slope;
// if (get_mA() > current_cap){
// voltage_override = 0;
// }
//}
unsigned long int ticks_diff(unsigned long int t2,unsigned long int t1){ //t2 should be after t1, this is for calculating clock times.
if (t2<t1){//t2 must have wrapped around after t1 was taken
return (4294967295-(t1-t2));
}
return (t2-t1);
}
float get_mA(){// this is the estimated current being drawn from the power supply, not the actual motor current which is a bit different
float x =0;
x = currentlf_now*motor.voltage_limit/24;
return 1000*((4.0384440932900223e-002)+3.4514090071108776e-002*x*30);// this is off by like 12 percent in some cases a polynomial of third order fits the data better but might flake out at higher than 500 mA so I didn't try it.
}
float D_controller(float mA, float rps){
static unsigned long int last_check = micros();
static unsigned long int last_call_clock = micros();
unsigned long int time_since_last_check = ticks_diff(micros(),last_check);
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
static float last_rps = rps;
static float last_mA=mA;
float time_since_last_call_s = 0;
float d_term = 0;
float rps_diff = 0;
float mA_diff = 0;
static float slope_here = 0;
float f_slope_here =0;
// float d_term = 0;
time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
if ((time_since_last_check)>2000){
last_check = micros();
rps_diff = rps*1000-last_rps*1000;
mA_diff = (mA*1000-last_mA*1000);
slope_here = rps_diff/mA_diff;// remember to go down in rps if the slope is positive.
last_rps = rps;
last_mA = mA;
}
if (slope_here > 1){
slope_here = 1;
}
if (slope_here < -1){
slope_here = -1;
}
f_slope_here = slope_filter(slope_here);
d_term = f_slope_here*d_gain*time_since_last_call_s*controller_polarity;
rps = rps+d_term;
// diagnostic stuff
Serial.print("mA_diff:");
Serial.print(mA_diff/1000);
Serial.print(",");
Serial.print("mA:");
Serial.print(mA/100);
Serial.print(",");
Serial.print("rps:");
Serial.print(rps/100);
Serial.print(",");
Serial.print("f_slope:");
Serial.print(f_slope_here);
Serial.print(",");
Serial.print("d:");
Serial.print(d_term/time_since_last_call_s);
Serial.print(",");
Serial.print("rps_diff:");
Serial.println(rps_diff/1000);
//
return rps;
}
void perturb_rps(){
static unsigned long int last_call_clock = micros();
float time_since_last_call_s = 0;
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
static int perturbs = 0;
static float perturbation = 0;
static int perturb_dir = 1;
time_since_last_call_s = float(t_between_calls)/1000000;
last_call_clock = micros();
if (perturbation > perturbation_amplitude){
perturb_dir = -1;
}
if (perturbation < 0-perturbation_amplitude){
perturb_dir = 1;
}
motor.target += perturb_rate*time_since_last_call_s*perturb_dir;
perturbation += perturb_rate*time_since_last_call_s*perturb_dir;
}
void adjust_voltage(){
static unsigned long int last_call_clock = micros();
unsigned long int t_between_calls = ticks_diff(micros(),last_call_clock);
float time_since_last_call_s = float(t_between_calls)/1000000;
float step_size = v_roc*time_since_last_call_s;
last_call_clock = micros();
// Serial.println(step_size);
if ((motor.voltage_limit - goal_v)>(step_size*1.5)){//voltage too high, and by this much
motor.voltage_limit -= step_size;
//Serial.println("voltage too high");
}
if ((goal_v - motor.voltage_limit)>(step_size*1.5)){//voltage too low
motor.voltage_limit += step_size;
//Serial.println("voltage too low");
}
if (motor.voltage_limit < 0.3){
motor.voltage_limit = 0.3;
}
if (motor.voltage_limit > 10){// just in case things get borked
motor.voltage_limit = 10;
}
}
void setup() {
Serial.begin(1000000);
Serial.println("test serial3");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
FOCModulationType::SinePWM;
motor.voltage_limit = 2; // [V]
motor.velocity_limit = 320; // [rad/s]
motor.controller = MotionControlType::velocity_openloop;
// init motor hardware
motor.init();
motor.voltage_limit = goal_v;
motor.target = 0;
for (int i=0;i<300;i++){
motor.target = i*90/300;
for (int h=0;h<500;h++){
motor.move();
currentlf_now = diff_filter(currentSense.getDCCurrent());
}
//setpoint = adjust_setpoint_down(get_mA());
// Serial.println(setpoint);
Serial.println(get_mA());
}
}
void loop() {
static unsigned long int loop_clock_in = millis();
unsigned long int loop_time = 0;
float loop_time_s = 0;
// unsigned long int inner_loop_time = 0;
loop_time = ticks_diff(millis(), loop_clock_in);
loop_clock_in=millis();
loop_time_s = float(loop_time)/1000;
for (int i=0;i<100;i++){ // shouldloop at about 37 khz on b-g431 board
for (int q=0;q<5;q++){
motor.move();
motor.move();
motor.move();
motor.move();
motor.move();
}
currentlf_now = currentSense.getDCCurrent();
currentlf_now = diff_filter(currentlf_now);
// setpoint = adjust_setpoint_down(get_mA());
}
adjust_voltage();
// controller_polarity = detect_polarity(get_mA(), motor.target);
motor.target = D_controller(get_mA(),motor.target);
perturb_rps();
// adjust_lsc_up();
// overcurrent_trip();
SerialComm();
}
It works better in some way but there is too much noise in the current signal really. It messes things up too much. There are problems when the RPS reverses it’s direction of wandering, you could synchronize the slope measurement on that but it still looks like it’s a bit slow. There is more lag than I thought there would be, apparently. It’s possible and instant phase correction, instead of correcting the frequency, would work, to regulate things, but that would probably make noise so not an option for me.
Looks like this could be done but it is a little laggy for some reason.
I think the method used to calculate the DC current is one of the main problems. Considerable noise introduced which can be filtered but filtering it makes things laggy. You would need a current sensor at the input of the hbridge or something I think, or at least that would be better. Or maybe with the right tuning and stuff this could be gotten working.