3145H MCL Lab
A full localization and path-following sandbox on the VEX Override field. The simulator runs three poses at once: where the robot is, what dead reckoning thinks, and what Monte Carlo Localization recovers from four distance sensors, then drives the real chassis along your path with either Ramsete or a LemLib-style PID follower.
Click the field to drop waypoints, drag them to reshape the path, right-click one to delete it. Everything is seeded, so the scrub bar replays the exact same run.
HUD
true pose
odometry
MCL estimate
particles
Generated Rust
src/routes/generated_path.rs//! tuning constants generated by the 3145H MCL Lab.
//!
//! radians and inches, matching `odometry::Pose`. headings use the robot
//! convention this crate uses everywhere: zero points down +y and grows
//! clockwise, the way the inertial sensor reports it.
//!
//! regenerate instead of hand-editing. the simulator is the source of truth
//! for these, and a value changed here no longer matches the run it was
//! tuned on.
#![allow(dead_code)]
/* ---------------- monte carlo localization ---------------- */
pub const MCL_PARTICLES: usize = 500;
/// spread of the starting particle cloud, inches.
pub const MCL_INIT_POS_STDDEV: f64 = 2.000;
/// spread of their starting headings, radians (5.00 deg).
pub const MCL_INIT_HEADING_STDDEV: f64 = 0.08727;
/// motion noise added to each particle per update, inches.
pub const MCL_MOTION_POS_NOISE: f64 = 0.300;
/// heading noise per update, radians (0.30 deg).
pub const MCL_MOTION_HEADING_NOISE: f64 = 0.00524;
/// the sigma the filter *assumes* for the distance sensors, inches. a tuning
/// knob, not a measurement: raise it and the filter trusts odometry over the
/// sensors.
pub const MCL_SENSOR_SIGMA: f64 = 3.000;
/// fraction of particles scattered wide each resample, so the filter can
/// recover from being lost instead of collapsing on a confident wrong answer.
pub const MCL_RESAMPLE_JITTER: f64 = 0.0200;
/// does the filter's expected-reading raycast include the goals, or only the
/// walls and what's bolted to them.
pub const MCL_SEE_GOALS: bool = true;
/* ---------------- distance sensors ---------------- */
/// mounts as `[forward offset, right offset, relative heading]`, inches and
/// radians, same order the readings go into `Mcl::update`.
pub const SENSOR_MOUNTS: [[f64; 3]; 4] = [
[ 6.500, 0.000, 0.0000], // front
[ -6.500, 0.000, -3.1416], // back
[ 0.000, -6.000, -1.5708], // left
[ 0.000, 6.000, 1.5708], // right
];
/// past this a reading tells you nothing and gets dropped, inches. a real v5
/// distance sensor tops out near 78.
pub const SENSOR_MAX_RANGE: f64 = 78.00;
/// measured hardware noise, one sigma, inches. not the same thing as
/// `MCL_SENSOR_SIGMA`: this is what the sensor actually does.
pub const SENSOR_NOISE: f64 = 0.600;
/// how often the sensors get polled, hz.
pub const SENSOR_RATE_HZ: f64 = 25.0;
/* ---------------- odometry ---------------- */
/// tracking wheel quantization, inches per tick.
pub const ODOM_ENCODER_TICK: f64 = 0.01000;
/// inertial sensor drift, radians per second (0.08 deg/s).
pub const ODOM_IMU_DRIFT: f64 = 0.001396;
/// inertial sensor per-sample noise, radians (0.03 deg).
pub const ODOM_IMU_NOISE: f64 = 0.000524;
/* ---------------- chassis ---------------- */
/// left to right wheel contact patches, inches.
pub const TRACK_WIDTH: f64 = 11.500;
/// drive wheel diameter, inches. not the tracking wheel.
pub const WHEEL_DIAMETER: f64 = 3.250;
/// wheel rpm after the external gear ratio.
pub const WHEEL_RPM: f64 = 450.0;
pub const MOTORS_PER_SIDE: usize = 3;
/// free speed at the wheel, inches per second. derived, but spelled out so a
/// follower doesn't have to recompute it every loop.
pub const MAX_WHEEL_SPEED: f64 = 76.5763;
/// turn rate with the wheels at full opposing speed, radians per second.
pub const MAX_TURN_RATE: f64 = 13.3176;
/* ---------------- motion profile ---------------- */
pub const PATH_MAX_SPEED: f64 = 60.00;
pub const PATH_MAX_ACCEL: f64 = 40.00;
/// radians per second (180.0 deg/s).
pub const PATH_MAX_TURN_RATE: f64 = 3.14159;
pub const PATH_CURVE_FACTOR: f64 = 1.000;
/* ---------------- followers ---------------- */
/// ramsete aggressiveness, 1/m^2. si units, so the follower has to convert
/// inches to metres before using it. see `motion::pursuit::ramsete`.
pub const RAMSETE_B: f64 = 2.000;
/// ramsete damping, dimensionless.
pub const RAMSETE_ZETA: f64 = 0.700;
/// pursuit gains in lemlib units: inches of error in, +-127 out, derivative
/// per 10ms loop rather than per second.
pub const PURSUIT_LATERAL_KP: f64 = 8.000;
pub const PURSUIT_LATERAL_KD: f64 = 15.000;
/// angular gains take *degrees* of error, again matching lemlib.
pub const PURSUIT_ANGULAR_KP: f64 = 3.000;
pub const PURSUIT_ANGULAR_KD: f64 = 20.000;
/// lookahead along the path, inches.
pub const PURSUIT_LOOKAHEAD: f64 = 9.000;
/// the seed the sim replayed this tuning with. useless to the robot, kept so
/// a run on the field can be matched back to a run in the browser.
pub const SIM_SEED: u32 = 1337;
// no trajectory built yet. draw a path on the field and hit "Build path".
// the route body comes out of the built profile, not the waypoints, because
// the velocity profile is the part that matters.