Go Down

Topic: Mobile robot Encoder problem (Read 1 time) previous topic - next topic

Maisa

Sep 03, 2016, 11:27 pm Last Edit: Sep 04, 2016, 11:41 pm by Maisa
Hi all,
I am working on estimating robot position and velocity by using its encoder here's the encoder properties:http://www.robotshop.com/en/12v-120-rpm-dc-motor-641.html
but always the velocity is zero , I do not have any idea what the problem is,Can anyone give me the solution?
Hereby, you will find an attached image, it is the wiring of the circuit.

Code: [Select]
// Define pin locations and delay time between each query for wheel encoding
#define RightEncoderPinA 4
#define RightEncoderPinB 2
#define LeftEncoderPinA  3
#define LeftEncoderPinB  6

// Motor pins
int leftMotor =  5;
int rightMotor = 9;

// Speed output variables
double leftspeedcmd = 0;
double rightspeedcmd = 0;

// Encoder counters
volatile unsigned int LeftEncoderPos = 0;
volatile unsigned int RightEncoderPos = 0;

// Sample time for velocity calculation.  Lower resolution encoders need a longer sample time for an accurate reading
#define DELAY_TIME 100

// Define physical constants of the wheel for wheel encoding
#define WHEEL_CIRCUMFERENCE 0.15079 // In meters
#define WHEEL_TICKS 768          // The number of encoder 'ticks' for a full wheel cycle


// Velocity stuff
double leftvelocity = 0;
double rightvelocity = 0;



void setup()
{
 InitializeHardware();
}

// Modify your target speeds here
void loop()
{
// these setpoints are in m/s
 
    analogWrite (rightMotor,0);
    analogWrite (leftMotor,255);
 
  //Serial.print("LeftEncoder Ticks: ");
  //Serial.print(LeftEncoderPos);
  Serial.print("RightEncoderB Ticks: ");
  Serial.print( RightEncoderPos);
 
 Serial.print("RV: ");
 Serial.print(rightvelocity);
 Serial.print("\t");
  //Serial.print("  LRevolutions: ");
  //Serial.print(LeftEncoderPinA/320);
  //Serial.print(LeftEncoderPinB/768);
   
 // Serial.print(" NewRPos : ");
  //Serial.print( NewRPos );
 
 
  Serial.print("\n");
}


// Initialize all hardware components
void InitializeHardware()
{
 // Set input pin state for the line sensors
  Serial.begin(9600);
 pinMode(LeftEncoderPinA, INPUT);
 pinMode(LeftEncoderPinB, INPUT);
 pinMode(RightEncoderPinA,INPUT_PULLUP);
 pinMode(RightEncoderPinB, INPUT_PULLUP);
pinMode(leftMotor,OUTPUT);
pinMode(rightMotor,OUTPUT);

 // This function sets the EncoderEvent() function to be called whenever a line detector detects a change
 attachInterrupt(0, RightEncoderEvent, CHANGE);
 attachInterrupt(1, LeftEncoderEvent, CHANGE);

}

// Release all hardware components
void ReleaseHardware()
{
   // Release the interrupt registration
   detachInterrupt(0);
   detachInterrupt(1);
 
}

// Where the magic happens


// Return the real-world vehicle speed (in meters per second)
void GetSpeeds()
{
       long NewLPos;
       long NewRPos;
       long OldLPos;
       long OldRPos;
       double LdVal = 0;
       double RdVal = 0;
     // Find the old and new encoder positions
     OldLPos = LeftEncoderPos;
       OldRPos = RightEncoderPos;
     delay(DELAY_TIME);
     NewLPos = LeftEncoderPos;
       NewRPos = RightEncoderPos;

     // If an integer overflow occurs, throw out the new value
     if(abs(NewLPos - OldLPos) < 60000)
           LdVal = ((double)(NewLPos - OldLPos) / (double)DELAY_TIME) * 1000.0;
       if(abs(NewRPos - OldRPos) < 60000)
           RdVal = ((double)(NewRPos - OldRPos) / (double)DELAY_TIME) * 1000.0;

     // Find the linear velocity
     double LangVel = LdVal / (double)WHEEL_TICKS;
     leftvelocity = (LangVel * WHEEL_CIRCUMFERENCE);

       double RangVel = RdVal / (double)WHEEL_TICKS;
     rightvelocity = (RangVel * WHEEL_CIRCUMFERENCE);

}


// Encoder events for the interrupt calls
void LeftEncoderEvent()
{
 if (digitalRead(LeftEncoderPinA) == HIGH)
 {
   if (digitalRead(LeftEncoderPinB) == LOW)
     LeftEncoderPos++;
   else
     LeftEncoderPos--;
 }
 else
 {
   if (digitalRead(LeftEncoderPinB) == LOW)
     LeftEncoderPos--;
   else
     LeftEncoderPos++;
 }
}

void RightEncoderEvent()
{
 if (digitalRead(RightEncoderPinA) == HIGH)
 {
   if (digitalRead(RightEncoderPinB) == LOW)
     RightEncoderPos++;
   else
     RightEncoderPos--;
 }
 else
 {
   if (digitalRead(RightEncoderPinB) == LOW)
     RightEncoderPos--;
   else
     RightEncoderPos++;
 }
}


the output:
Code: [Select]
RightEncoderB Ticks: 0 RV: 0.00
RightEncoderB Ticks: 3 RV: 0.00
RightEncoderB Ticks: 14 RV: 0.00
RightEncoderB Ticks: 26 RV: 0.00
RightEncoderB Ticks: 37 RV: 0.00
RightEncoderB Ticks: 49 RV: 0.00
RightEncoderB Ticks: 61 RV: 0.00
RightEncoderB Ticks: 72 RV: 0.00
RightEncoderB Ticks: 84 RV: 0.00
RightEncoderB Ticks: 95 RV: 0.00
RightEncoderB Ticks: 107 RV: 0.00
RightEncoderB Ticks: 119 RV: 0.00
RightEncoderB Ticks: 131 RV: 0.00
RightEncoderB Ticks: 143 RV: 0.00
RightEncoderB Ticks: 154 RV: 0.00
RightEncoderB Ticks: 166 RV: 0.00
RightEncoderB Ticks: 178 RV: 0.00
RightEncoderB Ticks: 190 RV: 0.00
RightEncoderB Ticks: 202 RV: 0.00
RightEncoderB Ticks: 213 RV: 0.00
RightEncoderB Ticks: 225 RV: 0.00
RightEncoderB Ticks: 237 RV: 0.00
RightEncoderB Ticks: 249 RV: 0.00
RightEncoderB Ticks: 261 RV: 0.00
RightEncoderB Ticks: 273 RV: 0.00
RightEncoderB Ticks: 285 RV: 0.00
RightEncoderB Ticks: 297 RV: 0.00
RightEncoderB Ticks: 309 RV: 0.00
RightEncoderB Ticks: 320 RV: 0.00
RightEncoderB Ticks: 332 RV: 0.00
RightEncoderB Ticks: 344 RV: 0.00
RightEncoderB Ticks: 356 RV: 0.00
RightEncoderB Ticks: 368 RV: 0.00
RightEncoderB Ticks: 380 RV: 0.00
RightEncoderB Ticks: 392 RV: 0.00
RightEncoderB Ticks: 404 RV: 0.00
RightEncoderB Ticks: 415 RV: 0.00
RightEncoderB Ticks: 427 RV: 0.0
RightEncoderB Ticks: 439 RV: 0.00
RightEncoderB Ticks: 451 RV: 0.00
RightEncoderB Ticks: 463 RV: 0.00
RightEncoderB Ticks: 475 RV: 0.00
RightEncoderB Ticks: 486 RV: 0.00
RightEncoderB Ticks: 498 RV: 0.00
RightEncoderB Ticks: 510 RV: 0.00
RightEncoderB Ticks: 522 RV: 0.00
RightEncoderB Ticks: 534 RV: 0.00
RightEncoderB Ticks: 545 RV: 0.0
RightEncoderB Ticks: 557 RV: 0.00
RightEncoderB Ticks: 569 RV: 0.00
RightEncoderB Ticks: 581 RV: 0.00
RightEncoderB Ticks: 593 RV: 0.00
RightEncoderB Ticks: 604 RV: 0.00
RightEncoderB Ticks: 616 RV: 0.00
RightEncoderB Ticks: 628 RV: 0.00
RightEncoderB Ticks: 640 RV: 0.00
RightEncoderB Ticks: 652 RV: 0.00
RightEncoderB Ticks: 664 RV: 0.00
RightEncoderB Ticks: 676 RV: 0.00
RightEncoderB Ticks: 688 RV: 0.00
RightEncoderB Ticks: 700 RV: 0.00
RightEncoderB Ticks: 711 RV: 0.00
RightEncoderB Ticks: 723 RV: 0.00
RightEncoderB Ticks: 735 RV: 0.00
RightEncoderB Ticks: 747 RV: 0.00
RightEncoderB Ticks: 759RV: 0.00
RightEncoderB Ticks: 771 RV: 0.00
RightEncoderB Ticks: 783 RV: 0.00
RightEncoderB Ticks: 795 RV: 0.00
RightEncoderB Ticks: 807 RV: 0.00
RightEncoderB Ticks: 819 RV: 0.00
RightEncoderB Ticks: 832 RV: 0.00
RightEncoderB Ticks: 844 RV: 0.00
RightEncoderB Ticks: 856 RV: 0.00
RightEncoderB Ticks: 868 RV: 0.00
RightEncoderB Ticks: 880 RV: 0.00
RightEncoderB Ticks: 892 RV: 0.00
RightEncoderB Ticks: 904 RV: 0.00
RightEncoderB Ticks: 916 RV: 0.00
RightEncoderB Ticks: 928 RV: 0.00
RightEncoderB Ticks: 940 RV: 0.00
RightEncoderB Ticks: 952 RV: 0.00
RightEncoderB Ticks: 964 RV: 0.00
RightEncoderB Ticks: 976 RV: 0.00
RightEncoderB Ticks: 988 RV: 0.00
RightEncoderB Ticks: 1000 RV: 0.00
RightEncoderB Ticks: 1012 RV: 0.00
RightEncoderB Ticks: 1024 RV: 0.00
RightEncoderB Ticks: 1036 RV: 0.00
RightEncoderB Ticks: 1048 RV: 0.00
RightEncoderB Ticks: 1061 RV: 0.00
RightEncoderB Ticks: 1073 RV: 0.00
RightEncoderB Ticks: 1085 RV: 0.00
RightEncoderB Ticks: 1097 RV: 0.00
RightEncoderB Ticks: 1109 RV: 0.00
RightEncoderB Ticks: 1121 RV: 0.00
RightEncoderB Ticks: 1133 RV: 0.00
RightEncoderB Ticks: 1145 RV: 0.00
RightEncoderB Ticks: 1157 RV: 0.00
RightEncoderB Ticks: 1169 RV: 0.00
RightEncoderB Ticks: 1181 RV: 0.00
RightEncoderB Ticks: 1193 RV: 0.00
RightEncoderB Ticks: 1205 RV: 0.00
RightEncoderB Ticks: 1217 RV: 0.00
RightEncoderB Ticks: 1229 RV: 0.00
RightEncoderB Ticks: 1241 RV: 0.00
RightEncoderB Ticks: 1253 RV: 0.00
RightEncoderB Ticks: 1265 RV: 0.00
RightEncoderB Ticks: 1278 RV: 0.00
RightEncoderB Ticks: 1290 RV: 0.00
RightEncoderB Ticks: 1302 RV: 0.00
RightEncoderB Ticks: 1314 RV: 0.00
RightEncoderB Ticks: 1326 RV: 0.00




Regards

jremington

#1
Sep 04, 2016, 02:55 am Last Edit: Sep 04, 2016, 02:56 am by jremington
Few people will download a .zip file. Please read "How to use this forum" and post the code using code tags.

Also, post a link to the encoder product page or data sheet, and a wiring diagram showing how you have connected it.

Southpark

#2
Sep 04, 2016, 04:56 am Last Edit: Sep 04, 2016, 04:57 am by Southpark
Hi all,
I am working on estimating robot position and velocity by using its encoder
Regards

Should always do the usual and get back to bare minimum basics. So start off with bare minimum code to test the most important things.... such as whether your arduino is receiving basic signals from the interrupt events (for rising edge signals etc).

Like, use 1 encoder only to begin with. Set up your pin and interrupt to detect signal changes for 1 channel (eg. channel A) of that encoder. Write code so that something gets printed onto the Serial Monitor whenever a rising edge is detected by the interrupt pin. Also, if you are using INTERNAL arduino pullup resistors for the interrupt pin, then use the "INPUT_PULLUP"  codeword for the software setup of the interrupt input pin.

Manually rotate the encoder and watch the serial monitor. If you start seeing activity when you manually turn the encoder, then that would be a good sign.

Maisa

Should always do the usual and get back to bare minimum basics. So start off with bare minimum code to test the most important things.... such as whether your arduino is receiving basic signals from the interrupt events (for rising edge signals etc).

Like, use 1 encoder only to begin with. Set up your pin and interrupt to detect signal changes for 1 channel (eg. channel A) of that encoder. Write code so that something gets printed onto the Serial Monitor whenever a rising edge is detected by the interrupt pin. Also, if you are using INTERNAL arduino pullup resistors for the interrupt pin, then use the "INPUT_PULLUP"  codeword for the software setup of the interrupt input pin.

Manually rotate the encoder and watch the serial monitor. If you start seeing activity when you manually turn the encoder, then that would be a good sign.
I have done what you said,the encoder detects the edges and in the last code also detects,because it increments or decrease if it's detects any change if it's rising or falling.So the basics is right,where's the problem ??any suggests?

Southpark

#4
Sep 05, 2016, 02:52 am Last Edit: Sep 07, 2016, 08:32 pm by Southpark
I have done what you said,the encoder detects the edges and in the last code also detects,because it increments or decrease if it's detects any change if it's rising or falling.So the basics is right,where's the problem ??any suggests?
Your function.... "GetSpeeds()"... it is a function in your code. But it never appears to be used (aka ..... is never 'called').

In other words, you forgot to use this function that is meant to calculate the value that you're interested in.


Maisa

Your function.... "GetSpeeds()"... it is a function in your code. But it never appears to be used (aka ..... is never 'called').

In other words, you forgot to use this function that is meant to calculate the value that you're interested in.


Yes,you're right.I got over the problem but I have another one ,the error of the pid always zero wihout any calibration,any suggestion?

Code: [Select]
// Define pin locations and delay time between each query for wheel encoding
#define RightEncoderPinA 4
#define RightEncoderPinB 2
#define LeftEncoderPinA  3
#define LeftEncoderPinB  6

// Motor pins
int leftMotor =  5;
int rightMotor = 9;

// Speed output variables
double leftspeedcmd = 0;
double rightspeedcmd = 0;

double righterror;

// Encoder counters
volatile unsigned int LeftEncoderPos = 0;
volatile unsigned int RightEncoderPos = 0;

// Sample time for velocity calculation.  Lower resolution encoders need a longer sample time for an accurate reading
#define DELAY_TIME 10

// Define physical constants of the wheel for wheel encoding
#define WHEEL_CIRCUMFERENCE 0.15079 // In meters
#define WHEEL_TICKS 768          // The number of encoder 'ticks' for a full wheel cycle


// PID Gains
double Kp = 20.0;
double Ki = 1.0;
double Kd = 0.2;
double maxint = 10.0;

long RdVal;
// time of last speed update
unsigned long LastUpdate = 0;

// memory for I and D terms
double lastlefterror = 0;
double lastrighterror = 0;
double lefti = 0;
double righti = 0;

// Wheel speed setpoints
double targetLvelocity = 0;
double targetRvelocity = 0;


// Velocity stuff
double leftvelocity = 0;
double rightvelocity = 0;


int maxspeed = 255;

void setup()
{
 InitializeHardware();
}

// Modify your target speeds here
void loop()
{
// these setpoints are in m/s

// these setpoints are in m/s
 targetLvelocity = 1;
 targetRvelocity = 1;
 SetWheelSpeeds();
 
    analogWrite (rightMotor,0);
    analogWrite (leftMotor,125);
 
  //Serial.print("LeftEncoder Ticks: ");
  //Serial.print(LeftEncoderPos);
  Serial.print("RightEncoderB Ticks: ");
  Serial.print( RightEncoderPos);
  Serial.print("\t");
 Serial.print("RV: ");
 Serial.print(rightvelocity);
 Serial.print("\t");
  //Serial.print("  LRevolutions: ");
  //Serial.print(LeftEncoderPinA/320);
  //Serial.print(LeftEncoderPinB/768);
  Serial.print("R_error");
  Serial.print(righterror);
  Serial.print("\t");
  Serial.print(" Right_value:");
  Serial.print("RdVal");
   Serial.print("\t");
 // Serial.print(" NewRPos : ");
  //Serial.print( NewRPos );
 
 
  Serial.print("\n");
}


// Initialize all hardware components
void InitializeHardware()
{
 // Set input pin state for the line sensors
  Serial.begin(9600);
 pinMode(LeftEncoderPinA, INPUT_PULLUP);
 pinMode(LeftEncoderPinB, INPUT_PULLUP);
 pinMode(RightEncoderPinA,INPUT_PULLUP);
 pinMode(RightEncoderPinB, INPUT_PULLUP);
pinMode(leftMotor,OUTPUT);
pinMode(rightMotor,OUTPUT);

 // This function sets the EncoderEvent() function to be called whenever a line detector detects a change
 attachInterrupt(0, RightEncoderEvent, CHANGE);
 attachInterrupt(1, LeftEncoderEvent, CHANGE);

}
// Release all hardware components
void ReleaseHardware()
{
   // Release the interrupt registration
   detachInterrupt(0);
   detachInterrupt(1);
 
}
// Return the real-world vehicle speed (in meters per second)
void SetWheelSpeeds()
{
 GetSpeeds();

 // find errors, dt
 double lefterror = (double)targetLvelocity-((double)leftvelocity);
 double righterror = (double)targetRvelocity-((double)rightvelocity*0);
 double dt = (micros() - LastUpdate);
 // convert to seconds
 dt /= 1000.0;
 // i
 lefti += lefterror * dt;
 righti += righterror * dt;
 // prevent integral windup
 lefti = constrain (lefti, -maxint, maxint);
 righti = constrain(righti, -maxint, maxint);
 // d
 double leftd = (lefterror - lastlefterror)/dt;
 double rightd =(righterror - lastrighterror)/dt;
 // sum
 leftspeedcmd = (Kp * lefterror) + (Ki * lefti) + (Kd * leftd);
 rightspeedcmd = (Kp * righterror) + (Ki * righti) + (Kd * rightd);
 // bounds check
 leftspeedcmd = constrain(leftspeedcmd, -maxspeed, maxspeed);
 rightspeedcmd = constrain(rightspeedcmd, -maxspeed, maxspeed);
 // output speeds
 SetSpeed(leftMotor, leftspeedcmd);
 SetSpeed(rightMotor, rightspeedcmd);
 
 // store info for next time
 LastUpdate = micros();
 lastlefterror = lefterror;
 lastrighterror = righterror; 
}

void GetSpeeds()
{
       long NewLPos;
       long NewRPos;
       long OldLPos;
       long OldRPos;
       double LdVal = 0;
       double RdVal = 0;
     // Find the old and new encoder positions
     OldLPos = LeftEncoderPos;
       OldRPos = RightEncoderPos;
     delay(DELAY_TIME);
     NewLPos = LeftEncoderPos;
       NewRPos = RightEncoderPos;

     // If an integer overflow occurs, throw out the new value
     if(abs(NewLPos - OldLPos) < 60000)
           LdVal = ((double)(NewLPos - OldLPos) / (double)DELAY_TIME) * 1000.0;
       if(abs(NewRPos - OldRPos) < 60000)
           RdVal = ((double)(NewRPos - OldRPos) / (double)DELAY_TIME) * 1000.0;

     // Find the linear velocity
     double LangVel = LdVal / (double)WHEEL_TICKS;
     leftvelocity = (LangVel * WHEEL_CIRCUMFERENCE);

       double RangVel = RdVal / (double)WHEEL_TICKS;
     rightvelocity = (RangVel * WHEEL_CIRCUMFERENCE);

}

// Encoder events for the interrupt calls
void LeftEncoderEvent()
{
 if (digitalRead(LeftEncoderPinA) == HIGH)
 {
   if (digitalRead(LeftEncoderPinB) == LOW)
     LeftEncoderPos++;
   else
     LeftEncoderPos--;
 }
 else
 {
   if (digitalRead(LeftEncoderPinB) == LOW)
     LeftEncoderPos--;
   else
     LeftEncoderPos++;
 }
}

void RightEncoderEvent()
{
 if (digitalRead(RightEncoderPinA) == HIGH)
 {
   if (digitalRead(RightEncoderPinB) == LOW)
     RightEncoderPos++;
   else
     RightEncoderPos--;
 }
 else
 {
   if (digitalRead(RightEncoderPinB) == LOW)
     RightEncoderPos--;
   else
     RightEncoderPos++;
 }
}

// Set the vehicle speed
void SetSpeed(int motor,  int newspeed)
{
 return;
// do speed setting stuff here, depends on your motor controller
}

jremington

#6
Sep 10, 2016, 05:20 pm Last Edit: Sep 10, 2016, 05:24 pm by jremington
There are many, many serious errors in that program. It can't possibly work.

You need to go through it line by line and make sure that you understand each line. Correct those that are wrong.

A few examples:
Code: [Select]
double righterror = (double)targetRvelocity-((double)rightvelocity*0);

Code: [Select]
double dt = (micros() - LastUpdate);
 // convert to seconds
 dt /= 1000.0;


Code: [Select]
// Set the vehicle speed
void SetSpeed(int motor,  int newspeed)
{
 return;
// do speed setting stuff here, depends on your motor controller
}

Maisa

There are many, many serious errors in that program. It can't possibly work.

You need to go through it line by line and make sure that you understand each line. Correct those that are wrong.

A few examples:
Code: [Select]
double righterror = (double)targetRvelocity-((double)rightvelocity*0);

Code: [Select]
double dt = (micros() - LastUpdate);
 // convert to seconds
 dt /= 1000.0;


Code: [Select]
// Set the vehicle speed
void SetSpeed(int motor,  int newspeed)
{
 return;
// do speed setting stuff here, depends on your motor controller
}

The first two examples you mentioned I put them to check where's the problem but I forgot to re_correct them.In the third example where's the problem?

jremington

#8
Sep 11, 2016, 03:37 am Last Edit: Sep 11, 2016, 03:37 am by jremington
Quote
In the third example where's the problem?
What is that function supposed  to do?

Go Up