0
$\begingroup$

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

$\endgroup$
4
  • $\begingroup$ in turn function, left motors are rotated anticlockwise and right motors are rotated clockwise ... shouldn't they all be rotating in the same direction? ... wheels rotate in opposite direction when moving forward, or backward, in a straight line $\endgroup$
    – jsotola
    Oct 20, 2022 at 15:20
  • $\begingroup$ Opposite side wheels have to rotate in opposite direction in order to pivot the mecanum wheeled robot. $\endgroup$
    – Ha-Naa
    Oct 20, 2022 at 15:35
  • $\begingroup$ It might be hard for our members to help debug your code without seeing more (all) of it. Please edit your question and add the rest of the code. Also, if you put some printf() statements in there, what do you see? Are your encoders reading properly? Are you calculating the total turned angle properly? ... $\endgroup$
    – Ben
    Oct 20, 2022 at 17:52
  • $\begingroup$ @Ha-Naa please look at diagram e) at en.m.wikipedia.org/wiki/Mecanum_wheel#Design ... all four wheels are turning anti-clockwise $\endgroup$
    – jsotola
    Oct 22, 2022 at 22:57

1 Answer 1

0
$\begingroup$

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.

$\endgroup$

Your Answer

By clicking โ€œPost Your Answerโ€, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.