I’m trying to use a B-G431B-ESC1 board to drive a fan motor quietly. I have been able to ascertain, by modifying the code and listening to the frequency, that there is a noise caused by the sensor update over I2C using an AS5600 sensor… I thought it was working with a different motor but not so much, probably the noise was being masked or something.
Looking at the code, it appears that the sensor.update() sits there and waits for the info to come back. This causes a pause in the rotation of the magnetic field and this leads to the noise. Surely this isn’t strictly necessary? If I could just sprinkle a couple motor.move(motor.target) command in there to update the commutation while it was waiting for the reply, I think that would work, but I don’t know how to call the motor.move function from within there… looked into using interrupts to run the motor.move() function at high frequency with good reliability, but that’s not promising for several reasons, there is probably stuff going on in there that you are not allowed to do inside an interrupt, and I am aware that’s generally not advisable to call a function like that inside an interrupt of course… I just need a smooth waveform, that’s totally paramount. The other code seems to run ok, fast enough it doesn’t cause any noticeable noise, as far as I can tell thus far. Just this one issue.
You will laugh at my code but it works better than torque mode, and basically does something similar. I do all this with great reluctance and as a last resort after trying and finding non-workable many other options for drivers. Even 2 pre-made chips couldn’t hack it.
I have some SPI sensors but would have to get the driver working for them. Think SPI might solve the problem, if this isn’t practical? The sensor has to be updated twice per rotation at least or the velocity calculation breaks down, found that out the hard way.
#include <SimpleFOC.h>
// NUMBER OF POLE PAIRS, NOT POLES
BLDCMotor motor = BLDCMotor(7);
// MUST USE 6PWM FOR B-G431 DRIVER
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
#define number_of_samples 10
float e=0;
float s=0;
float target_speed = 2;//rads per second
float accumulator =0;
float average =0;
int loop_counter = 0;
float phase_diff = 0.2;// just suppose it's 0.2 radians to start with, that seems to usually happens with 3 volts and 2 rads per second
int samples_taken = 0;
float no_load_phase_diff = 0;
float tim_advance =0;
// pid stuff
float p_gain = 1;
float i_gain = 0.00;
float d_gain = 0;
float control_signal_limit = 10;
float i_windup_limit = 2;
float setpoint = 2.3 ; //This determines the motor "timing" . Theoretically it should be 90 degrees but that doesn't work at all.
float control_signal = 0; // the control signal will be the *change* in velocity, it will be added to the current velocity and fed to the open loop commutation. in rads per second.
float i_term = 0;
float p_term = 0;
float d_term = 0;
float error = 0;
float pid_input=0;
unsigned long loop_time_last_clock_in = 0;
unsigned long loop_period = 0;
float samples[number_of_samples] = {0,0,0,0,0,0,0,0,0,0};
#define number_of_v_samples 10
float average_shaft_velocity = 0;
float radspersec = 0;
float shaft_v_samples[number_of_v_samples] = {0,0,0,0,0,0,0,0,0,0};// if you ever change the sample number this might break, it's a define way up at the top.
float shaft_velocity_accumulator = 0;
float shaft_velocity_f_func = 0;
void SerialComm()
{
switch(Serial.read())
{
case 'T': motor.target = Serial.parseFloat(); Serial.print("Speed target set");
case 't': Serial.print("Angular speed"); Serial.println(motor.target); break;
case 'V': motor.voltage_limit = Serial.parseFloat(); Serial.print("Voltage limit set");
case 'v': Serial.print("Voltage limit is:"); Serial.println(motor.voltage_limit); break;
case 'P': p_gain = Serial.parseFloat(); Serial.print("p_gain set");
case 'p': Serial.print("p_gain is:"); Serial.println(p_gain); break;
case 'I': i_gain = Serial.parseFloat(); Serial.print("i_gain set");
case 'i': Serial.print("i_gain is:"); Serial.println(i_gain); break;
case 'D': d_gain = Serial.parseFloat(); Serial.print("d_gain set");
case 'd': Serial.print("d_gain is:"); Serial.println(d_gain); break;
case 'O': i_windup_limit = Serial.parseFloat(); Serial.print("i_windup_limit set");
case 'o': Serial.print("windup limit is:"); Serial.println(i_windup_limit); break;
case 'U': setpoint = Serial.parseFloat(); Serial.print("setpoint changed:");
case 'u': Serial.print("setpoint is:"); Serial.println(setpoint); break;
}
}
float angle_diff(float a1,float a2){ //a2 should be the shaft, a1 is electrical so it'a like a1-a2. Thus if the angle is negative rotation is negative i.e. counterclockwise.
if (abs(a1-a2) > _PI){ // if it's more than 180 degrees difference and shaft is greater than electrical, the rotor will get pulled clockwise still
if ((a1-a2)<0){
return (_2PI-abs((a1-a2))); //then rotation is clockwise, so positive
}
}
if (abs(a1-a2) > _PI){ // ok so when greater than 180 degrees but shaft is less than electical then it will get pulled backwards, counterclockwise
if ((a1-a2)>0){
return (-1*(_2PI-abs((a1-a2))));// so it's negative
}
}
if (abs(a1-a2) < _PI){ // neither has rolled around yet if they are both less than 180 degrees. If shaft is less, rotation is clockwise so positive. If shaft is more, rotation is counterclockwise so negative
return (a1-a2);
}
}
unsigned long ticks_diff(unsigned long long time2, unsigned long time1){ // millis and micros() are both unsigned long integers. time 2 has to be later than time 1. time 2 would be millis() for instance. This is for use when you save a millis or micros time and then compare it later to detrmine elapsed time.
unsigned long ticks_diffed = 0;
if (time2 > time1){
ticks_diffed = time2-time1; // nothing rolled over recently so it's simple
return ticks_diffed;
}
if (time2 < time1){//micros or millis rolled over after time1 was saved
ticks_diffed = (4294964296-time1)+(time2); //current time plus the difference till the saved time would have rolled over too.
return ticks_diffed;
}
}
void pid_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 float last_input = 0;
// static long int last_clock_in = 0;
// long int time_elapsed = 0 ;
//we are using the global setpoint so it's easy to change without calling the function. idk that's the way it seems to be done usually.
error = setpoint-pid_input; // 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;
// i_term = i_term + (error*i_gain);// we actually have no use for an i term due to how we are using the output, as an acceleration, I think. But whatever.
//if (i_term > i_windup_limit){
// i_term = i_windup_limit;
// }
// if (i_term < (-1*i_windup_limit)){
// i_term = -i_windup_limit;
// }
//time_elapsed = ticks_diff(millis(), last_clock_in);
//last_clock_in = millis();
//d_term = d_gain*((pid_input-last_input)*1000/float(time_elapsed)); // this might not work right if the time elapsed is very large because you are trying to stuff a long unsigned int into a float. Multiply by 1000 so the time base is in seconds, not milliseconds.
// last_input = pid_input;
control_signal = p_term;// + i_term + d_term;
}
/*float shaft_angle_diff(float before, float now){ // assume difference is relatively small, less than pi radians, just factor in the modulo. It might be continous, not wrap around, idk.
static float diff = 0;
if (now > before){
diff = now - before;
}
if (now < before){// now must have wrapped around
diff = // assume before is within pi raidans of the wraparound point
}
}*/
void print_pid_stuff(){
// Serial.print("Pterm:");
//Serial.print(p_term);
// Serial.print(",");
// Serial.print("Iterm:");
// Serial.print(i_term);
// Serial.print(",");
// Serial.print("Dterm");
// Serial.println(d_term);
// Serial.print("Control:");
// Serial.print(control_signal);
// Serial.print(",");
//Serial.print("pid_input:");
// Serial.print(pid_input);
// Serial.print(",");
Serial.print("v:");
Serial.print(motor.shaftVelocity());
Serial.print(",");
// Serial.print("e:");
// Serial.print(e);
// Serial.print(",");
Serial.print("target_speed");
Serial.print(target_speed);
Serial.print(",");
// Serial.print("loop_period:");
//Serial.print(loop_period);
//Serial.print(",");
//Serial.print("");
// Serial.print(average);
// Serial.print(",");
Serial.print("sa:");
Serial.print((sensor.getAngle()));
Serial.print(",");
Serial.print("apd:");
Serial.println(average);
}
void print_other(){
Serial.print("phase_diff:");
Serial.print(phase_diff);
Serial.print(",");
Serial.print("s:");
Serial.print(s);
Serial.print(",");
Serial.print("e:");
Serial.println(e);
}
float no_load_phase_diff_measurement(){
long int start_time =0;
motor.target = 0.1;
loop_counter = 0;
start_time = millis();
while ((millis() - start_time)<6000){
for(int q = 0; q<40; q++){
for(int i = 0; i<1000; i++){
motor.move(motor.target);
}
sensor.update();
s = fmod(sensor.getAngle()*7,_2PI);//+(_2PI)
e = fmod(motor.shaft_angle*7,_2PI);
phase_diff=angle_diff(e,s);
samples[loop_counter%number_of_samples] = phase_diff;//store the last ten samples and calculate the average every time.
accumulator = 0;
for (int i = 0;i<number_of_samples;i++){
accumulator = accumulator+(samples[i]);
}
average = accumulator/number_of_samples;
loop_counter++;
print_pid_stuff();
}
}
no_load_phase_diff = average;
Serial.println("average phase displacement under no load measured");
return no_load_phase_diff;
}
void setup() {
sensor.init();
motor.voltage_sensor_align = 4;//I'm not even using the align info any more get rid of all that stuff later. No, keep it so I can use torque mode for acceleration.
Serial.begin(115200);
Serial.println("test serial");
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
motor.linkSensor(&sensor);
// link the motor and the driver
motor.linkDriver(&driver);
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// limiting motor movements
motor.voltage_limit = 7; // [V]
motor.velocity_limit = 520; // [rad/s]
// open loop control config
motor.controller = MotionControlType::velocity_openloop;
motor.useMonitoring(Serial);
motor.LPF_velocity.Tf = 0.05f;
// init motor hardware
motor.init();
motor.initFOC();
motor.target = 2;
motor.voltage_limit = 3;
no_load_phase_diff_measurement();
// shaft_velocity_check();
//start_motor(); // that was more about the old filtering approach which I no longer need.
}
void loop() {
for(int q =0; q<10;q++){
for (int x =0; x<7; x++){
for(int i = 0;i<200; i++){
motor.move(motor.target);
}
sensor.update();
}
s = (fmod((sensor.getAngle()*7)+no_load_phase_diff,_2PI));//+(_2PI)
e = fmod(motor.shaft_angle*7,_2PI);// motor.shaft_angle is actually the electricla angle, bad name
phase_diff=angle_diff(e,s);
samples[loop_counter%number_of_samples] = phase_diff;
accumulator = 0;
motor.move(motor.target);
for (int i = 0;i<number_of_samples;i++){
accumulator = accumulator+(samples[i]);
}
average = accumulator/number_of_samples;
pid_input = average;
pid_controller();
motor.move(motor.target);
target_speed = motor.shaftVelocity() + control_signal; // should normalize for the amount of time elapsed or things will change if you change loop time the effective gain will change. too complicated for now.
motor.target = target_speed;
if (loop_counter >(number_of_samples*10)){ //just so it never overflows, probably don't need this as it makes no diff if it overflows and wraps around.
loop_counter = 0;
}
loop_counter++;
}
//print_pid_stuff();
//print_other();
//SerialComm();
}
