Go Down

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

#### Maisa

##### Sep 03, 2016, 11:27 pmLast 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 pinsint leftMotor =  5;int rightMotor = 9;// Speed output variablesdouble leftspeedcmd = 0;double rightspeedcmd = 0;// Encoder countersvolatile 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 stuffdouble leftvelocity = 0;double rightvelocity = 0;void setup(){ InitializeHardware();}// Modify your target speeds herevoid 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 componentsvoid 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 componentsvoid 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 callsvoid 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.00RightEncoderB Ticks: 3 RV: 0.00RightEncoderB Ticks: 14 RV: 0.00RightEncoderB Ticks: 26 RV: 0.00RightEncoderB Ticks: 37 RV: 0.00RightEncoderB Ticks: 49 RV: 0.00RightEncoderB Ticks: 61 RV: 0.00RightEncoderB Ticks: 72 RV: 0.00RightEncoderB Ticks: 84 RV: 0.00RightEncoderB Ticks: 95 RV: 0.00RightEncoderB Ticks: 107 RV: 0.00RightEncoderB Ticks: 119 RV: 0.00RightEncoderB Ticks: 131 RV: 0.00RightEncoderB Ticks: 143 RV: 0.00RightEncoderB Ticks: 154 RV: 0.00RightEncoderB Ticks: 166 RV: 0.00RightEncoderB Ticks: 178 RV: 0.00RightEncoderB Ticks: 190 RV: 0.00RightEncoderB Ticks: 202 RV: 0.00RightEncoderB Ticks: 213 RV: 0.00RightEncoderB Ticks: 225 RV: 0.00RightEncoderB Ticks: 237 RV: 0.00RightEncoderB Ticks: 249 RV: 0.00RightEncoderB Ticks: 261 RV: 0.00RightEncoderB Ticks: 273 RV: 0.00RightEncoderB Ticks: 285 RV: 0.00RightEncoderB Ticks: 297 RV: 0.00RightEncoderB Ticks: 309 RV: 0.00RightEncoderB Ticks: 320 RV: 0.00RightEncoderB Ticks: 332 RV: 0.00RightEncoderB Ticks: 344 RV: 0.00RightEncoderB Ticks: 356 RV: 0.00RightEncoderB Ticks: 368 RV: 0.00RightEncoderB Ticks: 380 RV: 0.00RightEncoderB Ticks: 392 RV: 0.00RightEncoderB Ticks: 404 RV: 0.00RightEncoderB Ticks: 415 RV: 0.00RightEncoderB Ticks: 427 RV: 0.0RightEncoderB Ticks: 439 RV: 0.00RightEncoderB Ticks: 451 RV: 0.00RightEncoderB Ticks: 463 RV: 0.00RightEncoderB Ticks: 475 RV: 0.00RightEncoderB Ticks: 486 RV: 0.00RightEncoderB Ticks: 498 RV: 0.00RightEncoderB Ticks: 510 RV: 0.00RightEncoderB Ticks: 522 RV: 0.00RightEncoderB Ticks: 534 RV: 0.00RightEncoderB Ticks: 545 RV: 0.0RightEncoderB Ticks: 557 RV: 0.00RightEncoderB Ticks: 569 RV: 0.00RightEncoderB Ticks: 581 RV: 0.00RightEncoderB Ticks: 593 RV: 0.00RightEncoderB Ticks: 604 RV: 0.00RightEncoderB Ticks: 616 RV: 0.00RightEncoderB Ticks: 628 RV: 0.00RightEncoderB Ticks: 640 RV: 0.00RightEncoderB Ticks: 652 RV: 0.00RightEncoderB Ticks: 664 RV: 0.00RightEncoderB Ticks: 676 RV: 0.00RightEncoderB Ticks: 688 RV: 0.00RightEncoderB Ticks: 700 RV: 0.00RightEncoderB Ticks: 711 RV: 0.00RightEncoderB Ticks: 723 RV: 0.00RightEncoderB Ticks: 735 RV: 0.00RightEncoderB Ticks: 747 RV: 0.00RightEncoderB Ticks: 759RV: 0.00RightEncoderB Ticks: 771 RV: 0.00RightEncoderB Ticks: 783 RV: 0.00RightEncoderB Ticks: 795 RV: 0.00RightEncoderB Ticks: 807 RV: 0.00RightEncoderB Ticks: 819 RV: 0.00RightEncoderB Ticks: 832 RV: 0.00RightEncoderB Ticks: 844 RV: 0.00RightEncoderB Ticks: 856 RV: 0.00RightEncoderB Ticks: 868 RV: 0.00RightEncoderB Ticks: 880 RV: 0.00RightEncoderB Ticks: 892 RV: 0.00RightEncoderB Ticks: 904 RV: 0.00RightEncoderB Ticks: 916 RV: 0.00RightEncoderB Ticks: 928 RV: 0.00RightEncoderB Ticks: 940 RV: 0.00RightEncoderB Ticks: 952 RV: 0.00RightEncoderB Ticks: 964 RV: 0.00RightEncoderB Ticks: 976 RV: 0.00RightEncoderB Ticks: 988 RV: 0.00RightEncoderB Ticks: 1000 RV: 0.00RightEncoderB Ticks: 1012 RV: 0.00RightEncoderB Ticks: 1024 RV: 0.00RightEncoderB Ticks: 1036 RV: 0.00RightEncoderB Ticks: 1048 RV: 0.00RightEncoderB Ticks: 1061 RV: 0.00RightEncoderB Ticks: 1073 RV: 0.00RightEncoderB Ticks: 1085 RV: 0.00RightEncoderB Ticks: 1097 RV: 0.00RightEncoderB Ticks: 1109 RV: 0.00RightEncoderB Ticks: 1121 RV: 0.00RightEncoderB Ticks: 1133 RV: 0.00RightEncoderB Ticks: 1145 RV: 0.00RightEncoderB Ticks: 1157 RV: 0.00RightEncoderB Ticks: 1169 RV: 0.00RightEncoderB Ticks: 1181 RV: 0.00RightEncoderB Ticks: 1193 RV: 0.00RightEncoderB Ticks: 1205 RV: 0.00RightEncoderB Ticks: 1217 RV: 0.00RightEncoderB Ticks: 1229 RV: 0.00RightEncoderB Ticks: 1241 RV: 0.00RightEncoderB Ticks: 1253 RV: 0.00RightEncoderB Ticks: 1265 RV: 0.00RightEncoderB Ticks: 1278 RV: 0.00RightEncoderB Ticks: 1290 RV: 0.00RightEncoderB Ticks: 1302 RV: 0.00RightEncoderB Ticks: 1314 RV: 0.00RightEncoderB Ticks: 1326 RV: 0.00`

Regards

#### jremington

#1
##### Sep 04, 2016, 02:55 amLast Edit: Sep 04, 2016, 02:56 am by jremington

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 amLast 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

#3
##### Sep 04, 2016, 09:23 pm
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 amLast 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

#5
##### Sep 09, 2016, 11:31 pm
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 pinsint leftMotor =  5;int rightMotor = 9;// Speed output variablesdouble leftspeedcmd = 0;double rightspeedcmd = 0;double righterror;// Encoder countersvolatile 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 Gainsdouble Kp = 20.0;double Ki = 1.0;double Kd = 0.2;double maxint = 10.0;long RdVal;// time of last speed updateunsigned long LastUpdate = 0;// memory for I and D termsdouble lastlefterror = 0;double lastrighterror = 0;double lefti = 0;double righti = 0;// Wheel speed setpointsdouble targetLvelocity = 0;double targetRvelocity = 0;// Velocity stuffdouble leftvelocity = 0;double rightvelocity = 0;int maxspeed = 255;void setup(){ InitializeHardware();}// Modify your target speeds herevoid 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 componentsvoid 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 componentsvoid 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 callsvoid 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 speedvoid SetSpeed(int motor,  int newspeed){ return;// do speed setting stuff here, depends on your motor controller}`

#### jremington

#6
##### Sep 10, 2016, 05:20 pmLast 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 speedvoid SetSpeed(int motor,  int newspeed){ return;// do speed setting stuff here, depends on your motor controller}`

#### Maisa

#7
##### Sep 10, 2016, 11:43 pm
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 speedvoid 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 amLast 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