I haven’t made a video of the test at present, so let me describe the test process first.
1，The visual observation of motor speed is stable, but the measured value of speed is not stable, but note that the measured value of sensor angle is stable.
2，I looked at two documents MagneticSensor.cpp and BLDCMotor.cpp I have excerpted the parts related to speed measurement.
float BLDCMotor::shaftVelocity() {
float Ts = (_micros() - LPF_velocity.timestamp) * 1e-6;
// quick fix for strange cases (micros overflow)
if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
// calculate the filtering
float alpha = LPF_velocity.Tf/(LPF_velocity.Tf + Ts);
float vel = alpha*LPF_velocity.prev + (1-alpha)*sensor->getVelocity();
// save the variables
LPF_velocity.prev = vel;
LPF_velocity.timestamp = _micros();
return vel;
}
motor.shaftVelocity() -->It’s just “LPF.”_ velocity.Tf ”And the “ts” value makes a low-pass filter, but the object of this filter is “* Sensor - > getvelocity()”!
Then we’ll see“ sensor.getVelocity (), excerpt as follows:
float MagneticSensor::getVelocity(){
// calculate sample time
float Ts = (_micros() - velocity_calc_timestamp)*1e-6;
// quick fix for strange cases (micros overflow)
if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
// current angle
float angle_c = getAngle();
// velocity calculation
float vel = (angle_c - angle_prev)/Ts;
// save variables for future pass
angle_prev = angle_c;
velocity_calc_timestamp = _micros();
return vel;
}
Obviously, another calculation of “ts” value is carried out here. Do I think the second calculation of TS value is redundant?
Should it be quoted directly“ sensor.getVelocity () as motor.shaftVelocity () value?
Then I gave up“ motor.shaftVelocity (), change to direct use“ sensor.getVelocity ()”，
Result: the motor speed measurement is still unstable!!!
So I believe that in function“ sensor.getVelocity" Reference in“ sensor.getAngle () "is there something wrong?
Is the overflow processing part of “getangle()” incomplete?
My idea is as follows:
void getVelocity(){
static int data_sensor_0, real_sensor_data, d_sensor_data_0;
static float dt;
dt=(_micros()-dt)*1e-6;
int data_sensor = Sensor.read();
float d_sensor_data =data_sensor - d_sensor_data_0;
int data_overflow=16384;
int data_max = 0.80*data_overflow
if (fabs(d_sensor_data) >data_max){
if(data_sensor > data_sensor_0){
d_sensor_data = -(data_overflow -data_sensor+data_sensor_0);
}else if(data_sensor < data_sensor_0){
d_sensor_data = data_overflow -data_sensor_0 +data_sensor;
}
}
//low pass filter
d_sensor_data = (dt / (dt + LPF_velocity.Tf))*(d_sensor_data) + (1 - (dt / (dt +LPF_velocity.Tf)))*d_sensor_data_0;
d_sensor_data_0 = d_sensor_data;
data_sensor_0 = data_sensor;
real_sensor_data +=d_sensor_data;
float motor.velocity = d_sensor_data / dt;
float motor.position = real_sensor_data;
dt = _micros();
}
----I haven’t had time to test this part of the code, so I have no results for the time being!