On my attempt to try the closed loop example library in SimpleFOC, I encounter a problem with sensor support. It seems it doesn’t allow for Magnetic sensors with Sine+Cosine output, specifically the RM08D01 sensor. I could add an extra library to get the “getSensorAngle()” method without problems, like this:
MagneticSensorSineCos::MagneticSensorSineCos(uint8_t _pinSine, uint8_t _pinCos, float _offset){
pinSine = _pinSine;
pinCos = _pinCos;
offset = _offset;
}
void MagneticSensorSineCos::init(){
this->Sensor::init(); // call base class init
}
// Shaft angle calculation
// angle is in radians [rad]
float MagneticSensorSineCos::getSensorAngle(){
// raw data from the sensor
float cosine = ((float)(analogRead(pinCos)/ 1023.0f)*3.3f) - offset;
float sine = ((float)(analogRead(pinSine)/ 1023.0f)*3.3f) - offset;
float current_angle = atan2(sine,cosine) + _PI; // atan2 returns values in range [-PI, PI], we want [0, 2PI]
return current_angle;
}
Then, in my main loop I tried the sensor sketch to check the getAngle and getVelocity methods, the first works as intended, the second gets noisy. Idk what could lead to this behaviour, probably a hardware setup problem? Since the code in sensor.cpp was not modified, it should work with any sensor, as long as the time elapsed is higher than 100us.
void loop() {
sensor.update();
// display the angle and the angular velocity to the terminal
mySerial.println(sensor.getAngle());
mySerial.print("\t");
mySerial.println(sensor.getVelocity());
}
It’s written primarily for two linear halls sensing the rotor magnets directly, but it also works for sin/cos encoders if you set pole pairs to 1.
But that should give the same readings as your class, so it won’t fix the noisy velocity. It does get lowpass filtered in FOCMotor::shaftVelocity, so it may not be a problem for motor control. How noisy is it? And is the same noise present in the sin/cos readings? It may just be the inherent accuracy of the sensor and ADC.
Hi, dekutree64! Thank you for your prompt response. I will try this class anyway, just for curiosity. You are right about the inherent accuracy, like as you can see in the first image, the angle reading changes but veeeery little. I don’t know how valid is this, I’m sharing the image again but with read arrows that highlight the perturbance and hence the velocity noise…
Oh, I hadn’t noticed the text on the left side there was the readings. Indeed, those are some surprisingly large values. Try the printing code I posted in this thread the other day:
That will give us some pairs of angle and time values to get a better idea what’s going on.
Hi dekutree64!
Yes, I tried you code, but I’m afraid I couldn’t understand the source of the problem. In the end the issue is strangely the function “float Sensor::getVelocity()” in Sensor.cpp… and that is because I tried a code I asked to ChatGPT (I’m not ashamed of mentioned it xd) and now the readings are fairly good.
void loop() {
sensor.update();
// display the angle and the angular velocity to the terminal
//Serial.println(5*analogRead(A0)/1023.f);
//Serial.println(sensor.getAngle());
//Serial.print("\t");
//Serial.println(sensor.getSensorAngle());
float vel = getVelocity();
//Serial.print("Velocity (rad/s): ");
Serial.println(vel);
//Serial.println(getAcceleration());
delay(25); //25ms time between readings
}
float getVelocity() {
unsigned long now = millis();
float current_angle = sensor.getSensorAngle();
// Calculate delta time in seconds
float dt = (now - prev_time) / 1000.f;
// Avoid division by zero and bad first read
if (dt <= 0 || dt > 0.5) {
prev_angle = current_angle;
prev_time = now;
return velo; // Or return 0
}
// Compute velocity (rad/s)
float d_angle = current_angle - prev_angle;
// Handle wrap-around (angle jumping across 0 or 2π)
if (d_angle > PI) d_angle -= 2 * PI;
if (d_angle < -PI) d_angle += 2 * PI;
velo = (float)(d_angle / dt);
prev_angle = current_angle;
prev_time = now;
return velo;
}
Still, I clearly need to find an explanation about WHY the original function doesn’t work as it should. Anyway, the behaviour of the above function is depicted in the following image (still with noisy output but clearly better):
Do you know what is the best practice to compute the velocity? Or why does the results differs from the SimpleFOC library?
Edit: I found a hint, I tried the original getVelocity from SimpleFOC again, but this time with the 25ms time delay between readings. The noise dissapers a lot to the point it has the same behaviour as the code from GPT.
SimpleFOC getVelocity function without the 25ms delay:
This is a common issue that is not that much discussed within the docs. I’m not sure what MCU are you using, but it might be that its far too fast for velocitz calculation.
So we have a property in the sensor class that allows to only calculate the new velocity value after certain time, and not on each update call.
sensor.min_elapsed_time = 0.0001; // 100us by default
Basically setting the frequency of the velcoity calcualtion to 1/min_elapsed_time.
Try playing (raising) with this value and you should have better velocity readings.
Thank you so much for the hint, you were right, that variable was the key to get better measurements. I’m using an Arduino UNO to read the encoder, but originally, the STM32F4 is the one in the project which we are using to have a motor control. Anyway, I changed the min_elapsed_time to 0.08 secs and this is what I get:
