Inverse Kinematics of Differential Drive Robots and Odometry

Projects Programming
42

I think Mr @gcjr maybe i try to use differential drive to move smoothly between coordinate. but the robot can reach the goal coordinate accurately too. if that case which equation should i use for my robot ?

So the main problem here is, my robot can move in curve but he can determine his own way to get there. So i just input the goal coordinate.

And if i need to determine a path, it's not gonna work with the development in the future. In the future the robot can reach the goal coordinate and pass the interruption.

43

Thank you so much Mr @gcjr, Mr @jremington, Mr @Paul_KD7HB, @anon57585045 and all of people who want to help me to solve this problem. I just need to finish my final project to graduate from my univercity. Thank you so much.

I hope all people in here can help me until my problem is solved.

44

The simple approach is to compute the length each wheel must travel in it's curve. Divide each length by the minimum distance you can make each wheel move. The difference between the number of steps for each wheel is how often you need to add a step for the outside wheel or subtract a step for the inside wheel.

45

Great idea @Paul_KD7HB, Thaks for helping me, I'll try to implement your idea :grin::+1:t3:

46

Hello, I've build a Differential Robot with 2 Motors with encoder included, i have been make my robot move forward for 1 meter with PID controller and now i have seferal problem. i want my robot can move in more than 1 target on Coordinate (X,Y). For example :

TRAJECTORY TRACKING
TRAJECTORY TRACKING1056×816 3.35 KB

TRAJECTORY TRACKING CIRCLE
TRAJECTORY TRACKING CIRCLE1056×816 5.29 KB

Setup Hardware :

  1. 2 Motor DC PG28 with Encoder 7ppr and 16 gearbox ratio
  2. 2 Driver Motor BTS 7960 1 Pin PWM control and 2 Pin Dirrection Control
  3. 1 Arduino UNO R3

My Code :

#define ENCR 2 // WHITE
#define ENR 4 // BLACK
#define PWMR 10
#define INR2 6
#define INR1 7

#define ENCL 3 // GREEN
#define ENL 5 // BLUE
#define PWML 11
#define INL2 8
#define INL1 9
#define PI 3.1415926535897932384626433832795

// PARAMETER RODA
const float driveX = 100;   // cm
const float driveY = 100;   // cm
const int wheel_d = 80;           // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel circumference (mm) (KR)
const int counts_per_rev = 104;   // (7 pairs N-S) * (16 : 1 gearbox) = 224 (RR)
int power = 75;
int moff = 5;
int pwR = 20 ;
int pwL = 20 ;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

volatile int posR = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevTR = 0;
float eprevR = 0;
float eintegralR = 0;

volatile int posL = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevTL = 0;
float eprevL = 0;
float eintegralL = 0;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

//PARAMETER ODOMETRY
const float wheelDistance = 220;
float meanDistance = 0;
float theta = 0;
float posX = 0;
float posY = 0;
int x = 0;
int y = 0;
int deg = 0;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

/*
  // KEC 50
  // PID constants 1/10 = 0.1
  float kpR = 1.1;
  float kiR = 0.0008;
  float kdR = 0.016;

  float kpL = 1.1;
  float kiL = 0.0008;
  float kdL = 0.016;
*/

// KEC 75
// PID constants 1/10 = 0.1
float kpR = 0.68;
float kiR = 0.0008;
float kdR = 0.08;

float kpL = 0.7;
float kiL = 0.0008;
float kdL = 0.08;

void Odometry() {
  float distancePerCount = 2.08;

  static int encoderRPosPrev = 0;
  static int encoderLPosPrev = 0;

  float SR = distancePerCount * (posR - encoderRPosPrev);
  float SL = distancePerCount * (posL - encoderLPosPrev);

  encoderRPosPrev = posR;
  encoderLPosPrev = posL;

  x += SR * cos(theta);
  y += SL * sin(theta);
  theta += (SR - SL) / wheelDistance;

  meanDistance = (SL + SR) / 2;
  posX = posX + meanDistance * cos (theta);
  posY = posY + meanDistance * sin(theta);

  deg = theta * (180 / PI);
  Serial.print("  DEG : "); Serial.print(deg);
  Serial.print("  X : "); Serial.print(posX);
  Serial.print("  Y : "); Serial.print(posY);
  Serial.println();
  Diff(theta);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void Diff(long int theta) {
  float phiR = (cos(theta) + sin(theta));
  float phiL = (-sin(theta) + cos(theta));
  float M = (wheel_d / 2 * cos(theta));
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void setup() {
  Serial.begin(9600);
  pinMode(ENCR, INPUT);
  pinMode(ENR, INPUT);
  pinMode(ENCL, INPUT);
  pinMode(ENL, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCR), readEncoderR, RISING);
  attachInterrupt(digitalPinToInterrupt(ENCL), readEncoderL, RISING);

  pinMode(PWMR, OUTPUT);
  pinMode(INR1, OUTPUT);
  pinMode(INR2, OUTPUT);
  pinMode(PWML, OUTPUT);
  pinMode(INL1, OUTPUT);
  pinMode(INL2, OUTPUT);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void loop() {
  int drive = driveX + driveY;
  if (pwR && pwL <= power) {
    pwR += 5;
    pwL += 5;
  }

  // Calculate target number of ticks
  float num_rev = (drive * 10) / wheel_c;  // Convert to mm (RPSE)
  unsigned long target = num_rev * counts_per_rev;
  unsigned long tmm = (num_rev * wheel_c) / 10;

  setPID(target);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void setPID(int target) {


  // RIGHT WHEEL
  long currT = micros();  // time difference
  float deltaTR = ((float) (currT - prevTR)) / ( 1.0e6 ); prevTR = currT;
  float deltaTL = ((float) (currT - prevTL)) / ( 1.0e6 ); prevTL = currT;

  int posiR = 0; noInterrupts(); // disable interrupts temporarily while reading
  posiR = posR; interrupts(); // turn interrupts back on
  int posiL = 0; noInterrupts(); // disable interrupts temporarily while reading
  posiL = posL; interrupts(); // turn interrupts back on

  int eR = posR - target; // error
  int eL = posL - target; // error

  float dedtR = (eR - eprevR) / (deltaTR); // derivative
  eintegralR = eintegralR + eR * deltaTR; // integral
  float dedtL = (eL - eprevL) / (deltaTL); // derivative
  eintegralL = eintegralL + eL * deltaTL; // integral

  float uR = (kpR * eR) + (kiR * eintegralR) + (kdR * dedtR);  // control signal
  float uL = (kpL * eL) + (kiL * eintegralL) + (kdL * dedtL);  // control signal

  eprevR = eR; // store previous error
  eprevL = eL; // store previous error
  //---------------------------------------------------------------------------------------------------------------------------------------------------------//
  unsigned long num_ticks_r;
  unsigned long num_ticks_l;

  unsigned long diff_r;
  unsigned long diff_l;

  unsigned long enc_r_prev = posR;
  unsigned long enc_l_prev = posL;

  int pwrR = fabs(uR); // motor power
  int pwrL = fabs(uL); // motor power

  if ( pwrR && pwrL >= power ) {
    pwrR = pwR;
    pwrL = pwL;
  }

  int dirR = 1; // motor direction
  int dirL = 1; // motor direction
  if (uR && uL <= 0 ) {
    dirR = -1;
    dirL = -1;
  } else {
    dirR = 2;
    dirL = 2;
  }

  setMotorR(dirR, pwrR, PWMR, INR1, INR2);   // signal the motor
  setMotorL(dirL, pwrL, PWML, INL1, INL2);   // signal the motor

  Serial.print(" TARGET : "); Serial.print(target);
  Serial.print("R_L : "); Serial.print(posR);
  Serial.print("R_L : "); Serial.print(posL);
  Serial.print("|");
  Serial.print("P_R : "); Serial.print(pwrR);
  Serial.print("P_L : "); Serial.print(pwrL);
}

void setMotorR(int dirR, int pwmValR, int pwmR, int inR1, int inR2) {
  analogWrite(pwmR, pwmValR);
  if (dirR == 1) {
    digitalWrite(inR1, HIGH);
    digitalWrite(inR2, LOW);
  }
  else if (dirR == -1) {
    digitalWrite(inR1, LOW);
    digitalWrite(inR2, HIGH);
  }
  else if (dirR == 2) {
    digitalWrite(inR1, LOW);
    digitalWrite(inR2, LOW);
  }
}

void setMotorL(int dirL, int pwmValL, int pwmL, int inL1, int inL2) {
  analogWrite(pwmL, pwmValL);
  if (dirL == 1) {
    digitalWrite(inL1, HIGH);
    digitalWrite(inL2, LOW);
  }
  else if (dirL == -1) {
    digitalWrite(inL1, LOW);
    digitalWrite(inL2, HIGH);
  }
  else if (dirL == 2) {
    digitalWrite(inL1, LOW);
    digitalWrite(inL2, LOW);
  }
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void readEncoderR() {
  int E = digitalRead(ENR);
  if (E > 0) {
    posR++;
  }
  else {
    posR--;
  }
}

void readEncoderL() {
  int E = digitalRead(ENL);
  if (E > 0) {
    posL++;
  }
  else {
    posL--;
  }
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

Note :
for
const float driveX = 100; // cm
const float driveY = 100; // cm
and void Diff with the equation
its only test for my robot can drive on Coordinate (X,Y), but still not work.

Thank you !

47

Assuming your hardware is working ok, show us your code, turn functions etc.

You’ve already done the hard work, I’d suggest a struct and array of those structs, or a 2D array to define your points, then iterate through that list to move between points.

Fun project.
I’d also consider at least one reference point in the area of travel, that allows you to recalibrate your ‘known’ position occasionally - to overcome any wheel slip etc.

48

Hello @lastchancename, Thanks for reply, Okey i'll show my code.

Thanks for suggestion, can you explain and give me exaple for a struct and array of those structs, or a 2D array to define your points, then iterate through that list to move between points ? Thank you.

Setup Hardware :

  1. 2 Motor DC PG28 with Encoder 7ppr and 16 gearbox ratio
  2. 2 Driver Motor BTS 7960 1 Pin PWM control and 2 Pin Dirrection Control
  3. 1 Arduino UNO R3

My Code :

#define ENCR 2 // WHITE
#define ENR 4 // BLACK
#define PWMR 10
#define INR2 6
#define INR1 7

#define ENCL 3 // GREEN
#define ENL 5 // BLUE
#define PWML 11
#define INL2 8
#define INL1 9
#define PI 3.1415926535897932384626433832795

// PARAMETER RODA
const float driveX = 100;   // cm
const float driveY = 100;   // cm
const int wheel_d = 80;           // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel circumference (mm) (KR)
const int counts_per_rev = 104;   // (7 pairs N-S) * (16 : 1 gearbox) = 224 (RR)
int power = 75;
int moff = 5;
int pwR = 20 ;
int pwL = 20 ;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

volatile int posR = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevTR = 0;
float eprevR = 0;
float eintegralR = 0;

volatile int posL = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevTL = 0;
float eprevL = 0;
float eintegralL = 0;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

//PARAMETER ODOMETRY
const float wheelDistance = 220;
float meanDistance = 0;
float theta = 0;
float posX = 0;
float posY = 0;
int x = 0;
int y = 0;
int deg = 0;
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

/*
  // KEC 50
  // PID constants 1/10 = 0.1
  float kpR = 1.1;
  float kiR = 0.0008;
  float kdR = 0.016;

  float kpL = 1.1;
  float kiL = 0.0008;
  float kdL = 0.016;
*/

// KEC 75
// PID constants 1/10 = 0.1
float kpR = 0.68;
float kiR = 0.0008;
float kdR = 0.08;

float kpL = 0.7;
float kiL = 0.0008;
float kdL = 0.08;

void Odometry() {
  float distancePerCount = 2.08;

  static int encoderRPosPrev = 0;
  static int encoderLPosPrev = 0;

  float SR = distancePerCount * (posR - encoderRPosPrev);
  float SL = distancePerCount * (posL - encoderLPosPrev);

  encoderRPosPrev = posR;
  encoderLPosPrev = posL;

  x += SR * cos(theta);
  y += SL * sin(theta);
  theta += (SR - SL) / wheelDistance;

  meanDistance = (SL + SR) / 2;
  posX = posX + meanDistance * cos (theta);
  posY = posY + meanDistance * sin(theta);

  deg = theta * (180 / PI);
  Serial.print("  DEG : "); Serial.print(deg);
  Serial.print("  X : "); Serial.print(posX);
  Serial.print("  Y : "); Serial.print(posY);
  Serial.println();
  Diff(theta);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void Diff(long int theta) {
  float phiR = (cos(theta) + sin(theta));
  float phiL = (-sin(theta) + cos(theta));
  float M = (wheel_d / 2 * cos(theta));
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void setup() {
  Serial.begin(9600);
  pinMode(ENCR, INPUT);
  pinMode(ENR, INPUT);
  pinMode(ENCL, INPUT);
  pinMode(ENL, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCR), readEncoderR, RISING);
  attachInterrupt(digitalPinToInterrupt(ENCL), readEncoderL, RISING);

  pinMode(PWMR, OUTPUT);
  pinMode(INR1, OUTPUT);
  pinMode(INR2, OUTPUT);
  pinMode(PWML, OUTPUT);
  pinMode(INL1, OUTPUT);
  pinMode(INL2, OUTPUT);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void loop() {
  int drive = driveX + driveY;
  if (pwR && pwL <= power) {
    pwR += 5;
    pwL += 5;
  }

  // Calculate target number of ticks
  float num_rev = (drive * 10) / wheel_c;  // Convert to mm (RPSE)
  unsigned long target = num_rev * counts_per_rev;
  unsigned long tmm = (num_rev * wheel_c) / 10;

  setPID(target);
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void setPID(int target) {


  // RIGHT WHEEL
  long currT = micros();  // time difference
  float deltaTR = ((float) (currT - prevTR)) / ( 1.0e6 ); prevTR = currT;
  float deltaTL = ((float) (currT - prevTL)) / ( 1.0e6 ); prevTL = currT;

  int posiR = 0; noInterrupts(); // disable interrupts temporarily while reading
  posiR = posR; interrupts(); // turn interrupts back on
  int posiL = 0; noInterrupts(); // disable interrupts temporarily while reading
  posiL = posL; interrupts(); // turn interrupts back on

  int eR = posR - target; // error
  int eL = posL - target; // error

  float dedtR = (eR - eprevR) / (deltaTR); // derivative
  eintegralR = eintegralR + eR * deltaTR; // integral
  float dedtL = (eL - eprevL) / (deltaTL); // derivative
  eintegralL = eintegralL + eL * deltaTL; // integral

  float uR = (kpR * eR) + (kiR * eintegralR) + (kdR * dedtR);  // control signal
  float uL = (kpL * eL) + (kiL * eintegralL) + (kdL * dedtL);  // control signal

  eprevR = eR; // store previous error
  eprevL = eL; // store previous error
  //---------------------------------------------------------------------------------------------------------------------------------------------------------//
  unsigned long num_ticks_r;
  unsigned long num_ticks_l;

  unsigned long diff_r;
  unsigned long diff_l;

  unsigned long enc_r_prev = posR;
  unsigned long enc_l_prev = posL;

  int pwrR = fabs(uR); // motor power
  int pwrL = fabs(uL); // motor power

  if ( pwrR && pwrL >= power ) {
    pwrR = pwR;
    pwrL = pwL;
  }

  int dirR = 1; // motor direction
  int dirL = 1; // motor direction
  if (uR && uL <= 0 ) {
    dirR = -1;
    dirL = -1;
  } else {
    dirR = 2;
    dirL = 2;
  }

  setMotorR(dirR, pwrR, PWMR, INR1, INR2);   // signal the motor
  setMotorL(dirL, pwrL, PWML, INL1, INL2);   // signal the motor

  Serial.print(" TARGET : "); Serial.print(target);
  Serial.print("R_L : "); Serial.print(posR);
  Serial.print("R_L : "); Serial.print(posL);
  Serial.print("|");
  Serial.print("P_R : "); Serial.print(pwrR);
  Serial.print("P_L : "); Serial.print(pwrL);
}

void setMotorR(int dirR, int pwmValR, int pwmR, int inR1, int inR2) {
  analogWrite(pwmR, pwmValR);
  if (dirR == 1) {
    digitalWrite(inR1, HIGH);
    digitalWrite(inR2, LOW);
  }
  else if (dirR == -1) {
    digitalWrite(inR1, LOW);
    digitalWrite(inR2, HIGH);
  }
  else if (dirR == 2) {
    digitalWrite(inR1, LOW);
    digitalWrite(inR2, LOW);
  }
}

void setMotorL(int dirL, int pwmValL, int pwmL, int inL1, int inL2) {
  analogWrite(pwmL, pwmValL);
  if (dirL == 1) {
    digitalWrite(inL1, HIGH);
    digitalWrite(inL2, LOW);
  }
  else if (dirL == -1) {
    digitalWrite(inL1, LOW);
    digitalWrite(inL2, HIGH);
  }
  else if (dirL == 2) {
    digitalWrite(inL1, LOW);
    digitalWrite(inL2, LOW);
  }
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

void readEncoderR() {
  int E = digitalRead(ENR);
  if (E > 0) {
    posR++;
  }
  else {
    posR--;
  }
}

void readEncoderL() {
  int E = digitalRead(ENL);
  if (E > 0) {
    posL++;
  }
  else {
    posL--;
  }
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------//

Note :
for
const float driveX = 100; // cm
const float driveY = 100; // cm
and void Diff with the equation
its only test for my robot can drive on Coordinate (X,Y), but still not work.

Thank you !

49

Isn't this a continuation of the same topic from your previous thread?:
https://forum.arduino.cc/t/inverse-kinematics-of-differential-drive-robots-and-odometry/1006676

50

Hello @anon57585045, this is same problem but different topic. Because until now i can't implemented inverse kinematic in my robot. I try simplify my problem and get the simple solution more then use inverse kinematic for solving my problem. Thanks.

52

Generally members will often stop following split threads, because they don’t know if it’s already been resolved elsewhere.

I’d suggest you contact the forum moderators, and see if you can pull this together - then you’ll get more input .

53

Sure, thanks @lastchancename for your suggestion.

54

I have merged the two topics due to being essentially about the same subject and project and thus likely to either result in parallel discussions or cause helpers not following both to miss important context.

Best wishes for success with your project.
Per

55

Or they may have just given up on trying to help.

56

what about something like this

kine
kine789×490 11.4 KB

57

Yes that will be good enough, can you share the code @gcjr, Thank you so much

58

Because in my previous Topic, no one can help me anymore @pert

59

do you want help or simply the answer?

#include <stdio.h>
#include <math.h>
#include <unistd.h>

struct Pos {
    float   xPos;
    float   yPos;
    float   ang;
};

struct Bot {
    float   rad;
    float   fpsMax;
    float   fps;
    Pos     pos;
};

Pos     targ = { 15, 15, 90 };
Bot     bot  = { 9, 2 };

long    sec;


// -----------------------------------------------------------------------------
int
move (
    float   dSec,
    Bot    &bot )
{
    float  dD     = bot.fpsMax * dSec;

    bot.pos.xPos += dD * sin (bot.pos.ang * M_PI / 180);
    bot.pos.yPos += dD * cos (bot.pos.ang * M_PI / 180);

    printf ("  %s: %6.2f",   __func__, dD);

    return 0;
}

// -----------------------------------------------------------------------------
int
turn (
    float   dSec,
    Bot    &bot,
    float   ang )
{
    float dAng      = ang - bot.pos.ang;
    float degPerSec = bot.fpsMax * 180 / (M_PI * bot.rad);
    float deg       = degPerSec * dSec;

    if (deg > dAng)
        deg = dAng;

    float  angOff = -90;
    float  a0     = (bot.pos.ang + angOff) * M_PI / 180;
    bot.pos.ang  += deg;
    float  a1     = (bot.pos.ang + angOff) * M_PI / 180;

    bot.pos.xPos += bot.rad * (sin (a1) - sin (a0));
    bot.pos.yPos += bot.rad * (cos (a1) - cos (a0));


    printf ("  %s: %5.1f⁰",   __func__, ang);

    return 0;
}

// -----------------------------------------------------------------------------
int
dist (
    Pos  &a,
    Pos  &b )
{
    float dx = a.xPos - b.xPos;
    float dy = a.yPos - b.yPos;
    return sqrt (dx*dx + dy*dy);
}

// -----------------------------------------------------------------------------
int  moveCnt = 0;

int
step (
    float   dSec )
{
    sec += dSec;

    int   res  = 0;
    float d    = dist (bot.pos, targ); 
    float dx   = targ.xPos - bot.pos.xPos;
    float dy   = targ.yPos - bot.pos.yPos;
    float w    = atan2(dx, dy);
    float ang  = w * 180 / M_PI;

    printf ("%17s", "atan: ");
    printf (" w %5.3f", w);
    printf (" ang %5.3f", ang);
 // printf (" ( %5.3f, %5.3f)", targ.xPos, targ.yPos);
    printf (" ( %5.3f, %5.3f)", bot.pos.xPos, bot.pos.yPos);
    printf ("  %5.3f", dy / dx);
    printf ("\n");

    printf ("%s: %.1f, %.2f ang", __func__, d, ang);

    if (0 != d)  {
        if (0.5 < abs (ang - bot.pos.ang))  {
            turn (dSec, bot, ang);
            res =  1;
        }
        else  {
            move (dSec, bot);
            if (++moveCnt < 45)
                res = 1;
        }
    }

    if (0 == res)
        printf (", ( %5.3f, %5.3f)", targ.xPos, targ.yPos);
    else
        printf (", ( %5.3f, %5.3f)", bot.pos.xPos, bot.pos.yPos);
    printf (", %5.2f⁰", bot.pos.ang);
    printf ("\n");

 // sleep (1);
    return  res;
}


// -----------------------------------------------------------------------------
int
main (void)
{
    while (step(0.1))
        ;

    return 0;
}
60

Thank you so much @gcjr , i'll try to implementing the code in my system. If i need the answer i'll ask about how to implement your code in to my code. Because i just need help. i'll try it by my self. Thank you so much for helping me @gcjr and all of member and moderator. I'm so sorry for making all of you mad or maybe annoyed because i post so many Topic just for my project.

61

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.