Odometry 101 for FIRST Tech Challenge Robots

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:
https://cad.onshape.com/documents/5e3...
You will need an OnShape account which is free for educational purposes.

Omni wheels were purchased from RobotShop:
https://www.robotshop.com/en/60mm-alu...
REV encoders:
https://www.revrobotics.com/rev-11-1271/

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:
   • Behind the Bot FTC 11115 Gluten Free ...  

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
   • Odometry Spell Book Part 1: Building ...  
Part 2: Installing Odometers into Drivetrain
   • Odometry Spell Book Part 2: Installin...  
Part 3: Wiring an Odometry Drivetrain
   • Odometry Spell Book Part 3: Wiring an...  
Part 4: Understanding and Getting Started with the Odometry Software
   • Odometry Spell Book Part 4: Understan...  
Part 5: Learning To Move To Specific Positions
   • Odometry Spell Book Part 5: Learning ...  

FTC 18219 Primitive Data has launched a website for an OpenOdometry implementation:
https://openodometry.weebly.com/

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);
}