Turning a mecanum wheeled robot at a specific angle i.e 45 degree

Question

I am implementing inverse kinematics for motion control of a robot with four mecanum wheels. However I am not getting how to turn it at specific heading angle(45 degree)? I am using encoders to check if robot has really made a 45 degree turn. the problem is I am using following equation 𝜔𝑧= (−𝜔1 + 𝜔2 − 𝜔3 + 𝜔4)r/(4(𝑙𝑥+𝑙𝑦)) to compute turning angle from four encoders. but its not giving output as expected.

    volatile long eF1V = 0;
volatile long eF2V= 0;
volatile long eB1V= 0;
volatile long eB2V= 0;

int rpm_set=50;
float angle=0.0; 
float radB1=0.0; float radB2=0.0;float radF2=0.0;float radF1=0.0;
float linear_velocityB1=0.0; float linear_velocityB2=0.0; float linear_velocityF1=0.0; float linear_velocityF2=0.0;
int i=70;int rpmB2=0; int rpmB1=0; int rpmF1=0;int rpmF2=0;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
attachInterrupt(digitalPinToInterrupt(ENC_IN_FL), updateEF1, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_BL), updateEB1, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_FR), updateEF2, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_BR), updateEB2, RISING);

eF1V = 0;
 eF2V= 0;
 eB1V= 0;
 eB2V= 0;

}
void loop() {
   // put your main code here, to run repeatedly:
 
 while(angle<=0.78)
 {
 turn(); //left motors are tuning in one direction and right in opposite, both at PWM=105
 currentMillis=millis();
   if(currentMillis-previousMillis>interval)
   {
     previousMillis=currentMillis;
    rpmB2=(float)(encoderBRValue*60/ENC_COUNT_REV);   //rpm of rear wheel at right side
    radB2=rpmB2*0.10472; linear_velocityB2=radB2*0.1651;
    rpmB1=(float)(encoderBLValue*60/ENC_COUNT_REV);  
   radB1=rpmB1*0.10472; linear_velocityB1=radB1*0.1524;   //m/s
    rpmF2=(float)(encoderFRValue*60/ENC_COUNT_REV);   
    radF2=rpmF2*0.10472; linear_velocityF2=radF2*0.1651;
    rpmF1=(float)(eF1V*60/ENC_COUNT_REV);      
    radF1=rpmF1*0.10472; linear_velocityF1=radF1*0.1651;
    Serial.print("motorPwmFL=");Serial.print(i);Serial.print('\t');Serial.print("rpmF1=");Serial.println(rpmF1);
 angle=angle+(linear_velocityF2-linear_velocityF1+linear_velocityB2-linear_velocityB1)/(4*(lx+ly));
 Serial.print("angle=");Serial.println(angle);
 
 eF1V = 0;
  eF2V= 0;
  eB1V= 0;
  eB2V= 0;
   }
}
stop();
}

in turn function, left motors are rotated anticlockwise and right motors are rotated clockwise, all motors have same speed given by PWM signal. getanglefromencoders compute RPM of each wheel using encoder counts and then the above equation is used to calculate angle. but with this logic, my robot does not turn at 45 degree but it keeps rotating. Can someone please check this and guide me what I am doing wrong? Thanks

Answer 1

I'm not sure if this is the problem. (It would be really helpful to see the console output.) But in general, it is usually better to cast to a float before dividing.

For example, this is potentially problematic:

rpmB2=(float)(encoderBRValue*60/ENC_COUNT_REV);

If encoderBRValue is small, and ENC_COUNT_REV is large then the division might not be greater than 1. Integer division takes the floor of the result. i.e. it throws away the remainder. So it would be better to do this:

rpmB2=(float)encoderBRValue * 60.0f / ENC_COUNT_REV;

And actually, you don't even need the cast if you have 60.0f in there since that will cause the whole expression to be done in float math.