Context: I'm creating a map-replanner for a robot in C, it should add obstacles within the environment to the map depending on which IR-sensors are getting high readings.
For this i need to check how the robot is positioned IRL vs in the internal map (pointing north). But for some reason, i sometimes get negative values in the conversion. Any ideas what i'm doing wrong?
There should never be any negative values. The allowed values should lie between 0-360. As of now, i get negative values sometimes.
#define PI 3.14159265358979323846264338327950
#define DEG(x) (x*57.2957795130823208767981548141)
Code:
float currentRot = now.th; //current rotation-reading in RAD from function.
if(currentRot > PI){ //normalize values
currentRot = currentRot - 2 * PI;
}
if(currentRot < -PI){
currentRot = currentRot + 2 * PI;
}
currentRot = fmod(currentRot,PI*2); //convert into RAD-modular
currentRot = DEG(currentRot); //convert to degrees
Any ideas of what i'm doing wrong?