Odometry and sensors

Odometry runs in its own task every 10 ms once calibrate is called, using whatever combination of sensors you give it: tracking wheels on rotation sensors or ADI encoders, drive motor encoders, and the IMU. With no IMU, or one that fails calibration five times, it falls back to the wheels and rumbles --- on each failed attempt.

Tracking wheel sizesCopy Anchor Link

// rotation sensor or ADI encoder, diameter, offset from the tracking centre, gear ratio
let vertical = TrackingWheel::new(vertical_enc, Omniwheel::NEW_275, -2.5, 1.0);

Offsets are in inches: negative is left of centre for a vertical wheel and behind centre for a horizontal one.

Reading the poseCopy Anchor Link

let pose = chassis.get_pose();               // x, y in inches; theta in degrees
let rad = chassis.get_pose_with(true, true); // radians, standard position
chassis.set_pose(-60.0, 12.0, 90.0, false);
let speed = vectorlib::get_speed(false);     // inches/s and degrees/s

Correcting a drifting IMUCopy Anchor Link

If your inertial sensor reports 360° when the robot has really turned 358.6°, wrap it in ScalingImu (from TitanTron), which multiplies its heading and rotation:

let imu = ScalingImu::new(InertialSensor::new(peripherals.port_7), 1.00395);
let sensors = OdomSensors::new_custom(Some(vertical), None, Some(horizontal), None, Some(Box::new(imu)));