system
September 8, 2014, 12:27am
1
I had Rover 5 robot with dimensions below
Gearbox ratio: 86.8:1 Encoder type: Quadrature Encoder resolution: 1000 state changes per 3 wheel rotations
Actually from above information , I inferred these values below and I don't know how far I am accurate in that , so please need help
Wheel Diameter = 6 in CM
The distance between the center of the treads should be (225mm + 153mm) / 2. This gives 189mm. so
Track Width = 18.9 in CM
333.3 / 2 = 166.65 / 2 = 83.325 , so
CountsPerRevolution = 83.325
So the first question is , are the values computations above right ?
I want to use the information above to determine the odometry of robot so I used the code below but I don't if the code is will give accurate result
WheelDiameter = 6;
TrackWidth = 18.9;
CountsPerRevolution = 83;
DistancePerCount = (PI * WheelDiameter) / (double)CountsPerRevolution;
RadiansPerCount = DistancePerCount / TrackWidth;
long deltaLeft = _LeftEncoderTicks - _PreviousLeftEncoderCounts;
long deltaRight = _RightEncoderTicks - _PreviousRightEncoderCounts;
double deltaDistance = 0.5f * (double)(deltaLeft + deltaRight) * DistancePerCount;
double deltaX = deltaDistance * (double)cos(Heading);
double deltaY = deltaDistance * (double)sin(Heading);
double deltaHeading = (double)(deltaRight - deltaLeft) * RadiansPerCount;
X += deltaX;
Y += deltaY;
Heading += deltaHeading;
// limit heading to -Pi <= heading < Pi
if (Heading > PI)
{
Heading -= TwoPI;
}
else
{
if (Heading <= -PI)
{
Heading += TwoPI;
}
}
_PreviousLeftEncoderCounts = _LeftEncoderTicks;
_PreviousRightEncoderCounts = _RightEncoderTicks;
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
_LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
#else
_LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
#endif
}
// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_RightEncoderBSet = digitalReadFast(c_RightEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef RightEncoderIsReversed
_RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
#else
_RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
#endif
}
The second question is , is the code above will give accurate result of odometry?
system
September 8, 2014, 9:15am
2
The second question is , is the code above will give accurate result of odometry?
Not a chance.
WheelDiameter = 6;
TrackWidth = 18.9;
CountsPerRevolution = 83;
DistancePerCount = (PI * WheelDiameter) / (double)CountsPerRevolution;
RadiansPerCount = DistancePerCount / TrackWidth;
None
What's up with the stupid indenting?
system
September 8, 2014, 11:08am
3
PaulS:
The second question is , is the code above will give accurate result of odometry?
Not a chance.
WheelDiameter = 6;
TrackWidth = 18.9;
DistancePerCount = (PI * WheelDiameter) / (double)CountsPerRevolution;
None
of
these
variables
are
typed.
What's up with the stupid indenting?
thanks
What do you mean by your question ?
system
September 8, 2014, 4:57pm
7
I need help
With what? You posted poorly indented snippets of code. How do you expect us to guess whether you've defined variables with the correct type?
system
September 8, 2014, 5:32pm
8
PaulS:
I need help
With what? You posted poorly indented snippets of code. How do you expect us to guess whether you've defined variables with the correct type?
OK sorry for that . Actually I want to reach to a code without missing any ticks.
code below
#include "Arduino.h"
#include <digitalWriteFast.h>
// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 0
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 4
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks = 0;
#define PI 3.14159265
#define TwoPI 6.28318531
// Right encoder
#define c_RightEncoderInterrupt 1
#define c_RightEncoderPinA 3
#define c_RightEncoderPinB 5
volatile bool _RightEncoderBSet;
volatile long _RightEncoderTicks = 0;
double _DistancePerCount;
double _radiansPerCount;
//long _leftEncoderCounts;
long _PreviousLeftEncoderCounts;
// long _RightEncoderCounts;
long _PreviousRightEncoderCounts;
double X; // x coord in global frame
double Y; // y coord in global frame
double Heading; // heading (radians) in the global frame. The value lies in (-PI, PI]
double WheelDiameter;
double TrackWidth;
double CountsPerRevolution;
double DistancePerCount;
double RadiansPerCount;
void setup()
{
Serial.begin(9600);
WheelDiameter = 0.062;
TrackWidth = 0.189;
CountsPerRevolution = 83;
DistancePerCount = (PI * WheelDiameter) / (double)CountsPerRevolution;
RadiansPerCount = DistancePerCount / TrackWidth;
SetupEncoders();
delay(100);
}
void SetupEncoders()
{
// Quadrature encoders
// Left encoder
pinMode(c_LeftEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_LeftEncoderPinA, LOW); // turn on pullup resistors
pinMode(c_LeftEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_LeftEncoderPinB, LOW); // turn on pullup resistors
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
// Right encoder
pinMode(c_RightEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_RightEncoderPinA, LOW); // turn on pullup resistors
pinMode(c_RightEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_RightEncoderPinB, LOW); // turn on pullup resistors
attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
}
void loop()
{
long deltaLeft = _LeftEncoderTicks - _PreviousLeftEncoderCounts;
long deltaRight = _RightEncoderTicks - _PreviousRightEncoderCounts;
double deltaDistance = 0.5f * (double)(deltaLeft + deltaRight) * DistancePerCount;
double deltaX = deltaDistance * (double)cos(Heading);
double deltaY = deltaDistance * (double)sin(Heading);
double deltaHeading = (double)(deltaRight - deltaLeft) * RadiansPerCount;
X += deltaX;
Y += deltaY;
Heading += deltaHeading;
if (Heading > PI)
{
Heading -= TwoPI;
}
else
{
if (Heading <= -PI)
{
Heading += TwoPI;
}
}
_PreviousLeftEncoderCounts = _LeftEncoderTicks;
_PreviousRightEncoderCounts = _RightEncoderTicks;
Serial.print(_LeftEncoderTicks);
Serial.print("\t");
Serial.print(_RightEncoderTicks);
Serial.print("\n");
Serial.print("\n");
delay(200);
}
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
_LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
#else
_LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
#endif
}
// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_RightEncoderBSet = digitalReadFast(c_RightEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef RightEncoderIsReversed
_RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
#else
_RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
#endif
}
system
September 8, 2014, 5:37pm
9
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
What, exactly, is attached to these two pins?
system
September 8, 2014, 5:47pm
10
PaulS:
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
What, exactly, is attached to these two pins?
#define c_LeftEncoderInterrupt 0#define c_RightEncoderInterrupt 1
system
September 8, 2014, 6:07pm
11
I know what the values are. What, physically, is connected to the pins?
system
September 8, 2014, 6:56pm
12
PaulS:
only these pins are connected physically to encoder wires as follow. :
#define c_LeftEncoderPinA 2 // connected to signal A of left encoder (White wire )#define c_LeftEncoderPinB 4 // connected to signal B of left encoder (yellow wire )
#define c_RightEncoderPinA 3 // connected to signal A of right encoder (White wire )#define c_RightEncoderPinB 5 //connected to signal B of right encoder (yellow wire )
.......
