0
$\begingroup$

I am using an encoder to calculate the position and orientation of a mobile robot. The robot has two-wheel and each wheel is attached with an encoder, the result I am getting for my position x,y and theta is confusing. please I need help

// Pin definitions.
// - enc_a is ENC Signal A line (Arduino digital pin 2)
// - enc_b is ENC Signal B line (Arduino digital pin 3)
#define ENC_A1  2
#define ENC_B1  3

#define ENC_A2  18
#define ENC_B2  19

// Main loop refresh period.
#define REFRESH_MS  50

// Main serial data connection to computer.
#define BAUD_RATE   115200

// Encoder signal line states
volatile boolean state_a1 = 0;
volatile boolean state_b1 = 0;

// Encoder signal line states
volatile boolean state_a2 = 0;
volatile boolean state_b2 = 0;

// Encoder position
volatile int enc_pos_1 = 0;
int enc_pos_prev_1 = 0;
int enc_pos_change_1 = 0;

volatile int enc_pos_2 = 0;
int enc_pos_prev_2 = 0;
int enc_pos_change_2 = 0;

// normally on a differential drive system to when moving
// forwards the wheels are rotating in opposite directions
int encoder_direction_left = 1;
int encoder_direction_right = 1;

// Timing
unsigned long micros_current = 0;
unsigned long micros_prev = 0;
long micros_change = 0;

int dleft = 0;
int dright = 0;

float pi = 3.14;

float diameter = 42;                                    //(mm)

int count_per_revolution = 12;                          //(mm)

int Lwheel_c = 140;                                      //(mm)
int Rwheel_c = 139.9;                                       //(mm)

float Rright = 0.00;                                       // radius of curvature of right wheel

float Rleft = 0.00;                                      //Radius of curvature of left wheel

float Rc = 0.00 ;                                     //Radius of curvature

double theta = 0.00;

float phi = 0.00;

float RADS =  57.2958 ;

float dtheta = 0.00;

float beta = 0.00;                                     // orientatiom

double  x= 0.00;

int y = 0.00;

float dx = 0.00;

float dy = 0.00;

//float d_ori = 0.00;

int wheelbase = 91;                                   // wheelbase

void setup() {
    pinMode(ENC_A1, INPUT);
    pinMode(ENC_B1, INPUT); 

    pinMode(ENC_A2, INPUT);
    pinMode(ENC_B2, INPUT); 

    state_a1 = (boolean) digitalRead(ENC_A1);
    state_b1 = (boolean) digitalRead(ENC_B1);

    state_a2 = (boolean) digitalRead(ENC_A2);
    state_b2 = (boolean) digitalRead(ENC_B2);

    attachInterrupt(0, interrupt_enc_a1, CHANGE);
    attachInterrupt(1, interrupt_enc_b1, CHANGE);

    attachInterrupt(5, interrupt_enc_a2, CHANGE);
    attachInterrupt(4, interrupt_enc_b2, CHANGE); 

    micros_prev = micros();

    Serial.begin(BAUD_RATE);
}

void loop() {
    // Calculate elapsed time  
    micros_current = micros();
    if (micros_current < micros_prev) {
        micros_change = micros_current + (4294967295 - micros_prev);
    } else {
        micros_change = micros_current - micros_prev;
    }

    // Calculate change in encoder1 position.
    enc_pos_change_1 = enc_pos_1 - enc_pos_prev_1;
    // enc_pos_change_1 = abs(enc_pos_change_1);

    // Calculate change in encoder1 position.
    enc_pos_change_2 = enc_pos_2 - enc_pos_prev_2;
    // enc_pos_change_2 = abs(enc_pos_change_2);

    // Emit data
    Serial.print(enc_pos_1);
    Serial.print("\t");
    Serial.print(enc_pos_2);
    Serial.print("\t");
    Serial.print(dright);
    Serial.print("\t");
    Serial.print(dleft);
    Serial.print("\t");
    Serial.print(x);
    Serial.print("\t");
    Serial.print(y);
    Serial.print("\t");
    Serial.print(theta);
    Serial.print("\t");
    //Serial.print(enc_pos_change_2);
    Serial.print("\t");
    //Serial.print(micros_current);
    Serial.print("\t");
    //Serial.print(micros_change);
    Serial.print("\t");
    //Serial.print(enc_pos_change_1 / (micros_change / 1e6));
    Serial.print("\t");
    //Serial.print(enc_pos_change_2 / (micros_change / 1e6));
    Serial.print("\n");

    enc_pos_prev_1 = enc_pos_1;

    enc_pos_prev_2 = enc_pos_2;

    micros_prev = micros_current;

    //dleft = (enc_pos_1 * Lwheel_c)/count_per_revolution;

    //dright = (enc_pos_2 * Lwheel_c)/count_per_revolution;

    dleft = (enc_pos_change_1 * encoder_direction_left) * Lwheel_c/count_per_revolution;

    dright = (enc_pos_change_2 * encoder_direction_right)* Rwheel_c/count_per_revolution;

    // add the change in angle to the previous angle


    // constrain _theta to the range 0 to 2 pi

    if (theta > 2.0 * pi) 
    theta -= 2.0 * pi;
    else if (theta < -2.0*pi)
    theta += 2.0 * pi;

    Rc = (dleft + dright)/2;

    theta = (dleft - dright)/wheelbase;

    x = x + Rc * cos(theta);

    y = y + Rc * sin(theta);

    delay(REFRESH_MS);
}

// Detect pulses from depth encoder.

void interrupt_enc_a1() {
    if (!state_a1) {
        state_b1 ? enc_pos_1++: enc_pos_1--;         
    }
    state_a1 = !state_a1;
}

void interrupt_enc_b1() {
    state_b1 = !state_b1;
}

// Detect pulses from depth encoder.

void interrupt_enc_a2() {
    if (!state_a2) {
        state_b2 ? enc_pos_2++: enc_pos_2--;         
    }
    state_a2 = !state_a2;
}

void interrupt_enc_b2() {
    state_b2 = !state_b2;
}
$\endgroup$
  • $\begingroup$ can you explain more about what encoders are you using? are they optical encoders? magnetic encoders or something else? $\endgroup$ – Muhammad Idrees Mar 12 at 15:13
  • 1
    $\begingroup$ Welcome to Robotics oluwayinka esan. When writing a question you can use a code block to format your code nicely. At the top of the edit panel, look for the {} icon. Just select the text which is code and click the icon. I've done this for you this time, but it will make it much easier to read your future questions. Could you edit your question to add details of what x,y position you are getting and what you expected. $\endgroup$ – Mark Booth Mar 12 at 17:11
  • $\begingroup$ Also, if you edit your question to explain why you define x as a double, and y as an int (with a float literal!), that would also be useful. $\endgroup$ – Mark Booth Mar 12 at 17:12
  • $\begingroup$ I would suggest using double for all floating point values, and int or long for integer values. $\endgroup$ – NomadMaker Mar 13 at 10:02
  • $\begingroup$ Hi oluwayinka esan. I see you have some logic in your interrupt routines that count the encoder pulses. I'm wondering if that is the cause. What is that code doing? Generally I would just have those interrupt routines strictly count pulses. The higher level code that decides if the motor is going forward or backward can deal with the 'sign' of the pulse. Higher level code than that would deal with wheel circumference. And finally highest level code would deal with inverse kinematics to get pose. I think it would help to structure these layers so you can test each one. $\endgroup$ – Ezward Mar 13 at 21:43

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.