Hello!
First time poster here so I apologize if I make a mistake in where I am supposed to post this information.
We are trying to implement odometry to track the position of our robot during autonomous for the FTC 2021 competition. The tracking of the x and y positions work fine when the robot does not turn, but after any turn, the values are no longer correct. The values for the heading of the robot remain correct throughout.
Below is what each variable represents:
x - The x coordinate of the robot
y - The y coordinate of the robot
Theta - The heading of the robot from its initial position in radians, not limited to between any values (eg. can be above 2Pi and below -2Pi)
dx - Change in x coordinate between two instances of when odometry method is called
dy - Change in y coordinate between two instances of when odometry method is called
dTheta - Change in theta between two instances of when odometry method is called
lEncoderi and lEncoderf - The initial and final values of the left encoder in ticks
rEncoderi and rEncoderf - The initial and final values of the right encoder in ticks
bEncoderi and bEncoderf - The initial and final values of the back encoder in ticks
The left and right encoders are placed parallel to each other and measure the x and heading change.
The back encoder is placed perpendicular to those two encoders and measure the y change.
odometryCir - A constant to represent the circumference of our odometry wheel
odometryTick - A constant to represent the number of ticks per revolution of the encoders
SideOdometryToCentre - A constant that represents half the distance between the left and right encoder wheels
SideOdometryAngleFromCentre - A constant that represents the angle between the left and right encoder wheels, and the centre of the three encoder wheel setup
lengthFromOdometrySideToFront - A constant that represents the distance between the back encoder wheel and the centre of the three encoder wheel setup
The methods leftEncoder(), rightEncoder() and backEncoder() just return their respective encoder’s ticks
The following is the odometry code:
public class MecanumSubsystem extends Specifications{
private DcMotor leftBack;
private DcMotor rightBack;
private DcMotor leftForward;
private DcMotor rightForward;
public double x = 0;
public double y = 0;
public double Theta = 0;
private double dx = 0;
private double dy = 0;
public double dTheta = 0;
private int lEncoderi = 0;
private int rEncoderi = 0;
private int bEncoderi = 0;
private int lEncoderf = 0;
private int rEncoderf = 0;
private int bEncoderf = 0;
//Constants in the calculation of dx, dy, and dTheta
private double dxc = odometryCir/(odometryTick*2);
private double dThetac = -odometryCir/(odometryTick*2*SideOdometryToCentre*Math.cos(sideOdometryAngleFromCentre));
private double dyc = odometryCir/odometryTick;
public void OdometryProcess(){
lEncoderf = leftEncoder();
rEncoderf = rightEncoder();
bEncoderf = backEncoder();
dx = dxc*((lEncoderf-lEncoderi)+(rEncoderf-rEncoderi));
dTheta = dThetac*((rEncoderf-rEncoderi)-(lEncoderf-lEncoderi));
dy = (dyc*(bEncoderf-bEncoderi))+(lengthFromOdometrySideToFront*dTheta);
x = x+(dx*Math.cos(Theta))-(dy*Math.sin(Theta));
y = y+(dy*Math.cos(Theta))+(dx*Math.sin(Theta));
Theta += dTheta;
rEncoderi = rEncoderf;
lEncoderi = lEncoderf;
bEncoderi = bEncoderf;
}
From tests run, the x and y values change when the robot turns on the spot.
In a 90 degree turn, the robot is consistently off by around 30cm. All other distances, circumferences, etc. in the code are measured in cm.
If anyone could give some insight on this problem, that would be great as we have been struggling with it for several weeks at this point.