A deep dive into the math of a simple odometry implementation for FIRST Tech Challenge robots that was developed before and during the 20/21 FTC Ultimate Goal season.
Here is our hardware design for the Odopods:
[ Ссылка ]
You will need an OnShape account which is free for educational purposes.
Omni wheels were purchased from RobotShop:
[ Ссылка ]
REV encoders:
[ Ссылка ]
0:00 - Intro
0:49 - Skystone autonomous without odometry wheels
1:24 - Gluten Free Inspiration
1:50 - Building the odometry hardware
3:55 - Hardware overview
5:43 - The math behind odometry
20:19 - The odometry code
24:54 - A Test program and some calibration
27:18 - Autonomous run with odometry
The FUN interview with Steven and Peter from FTC 11115 Gluten Free at the 2019 MTI:
[ Ссылка ]
There is an excellent video series done by FTC 9794 Wizzards.exe called the Odometry Spell Book:
Part 1: Part 1: Building a Modified goBILDA Strafer Chassis Kit
[ Ссылка ]
Part 2: Installing Odometers into Drivetrain
[ Ссылка ]
Part 3: Wiring an Odometry Drivetrain
[ Ссылка ]
Part 4: Understanding and Getting Started with the Odometry Software
[ Ссылка ]
Part 5: Learning To Move To Specific Positions
[ Ссылка ]
FTC 18219 Primitive Data has launched a website for an OpenOdometry implementation:
[ Ссылка ]
On popular demand, the actual code in our main odometry function (you will have to define the variables and do the initialization yourself):
public void odometry() {
oldRightPosition = currentRightPosition;
oldLeftPosition = currentLeftPosition;
oldAuxPosition = currentAuxPosition;
currentRightPosition = -encoderRight.getCurrentPosition();
currentLeftPosition = encoderLeft.getCurrentPosition();
currentAuxPosition = encoderAux.getCurrentPosition();
int dn1 = currentLeftPosition - oldLeftPosition;
int dn2 = currentRightPosition - oldRightPosition;
int dn3 = currentAuxPosition - oldAuxPosition;
// the robot has moved and turned a tiny bit between two measurements:
double dtheta = cm_per_tick * ((dn2-dn1) / (LENGTH));
double dx = cm_per_tick * ((dn1+dn2) / 2.0);
double dy = cm_per_tick * (dn3 + ((dn2-dn1) / 2.0));
telemetrydx = dx;
telemetrydy = dy;
telemetrydh = dtheta;
// small movement of the robot gets added to the field coordinate system:
pos.h += dtheta / 2;
pos.x += dx * Math.cos(pos.h) - dy * Math.sin(pos.h);
pos.y += dx * Math.sin(pos.h) + dy * Math.cos(pos.h);
pos.h += dtheta / 2;
pos.h = normDiff(pos.h);
}
