Skip to main content

2 - Odometry

Initialization​

// Define rotation sensors for odometry wheels
RotationSensor verticalOdomWheel = RotationSensor("VerticalOdomWheel", 10);
RotationSensor horizontalOdomWheel = RotationSensor("HorizontalOdomWheel", 11);

// Define the radius of the dead wheels (in inches)
static constexpr double DEAD_WHEEL_RADIUS = 1.0; // in

// Create a perpendicular sensor odometry object
// (Since PerpendicularSensorOdometry is an AsyncTask, we must use a shared pointer)
PerpendicularSensorOdometry odometry = PerpendicularSensorOdometry(
    verticalOdomWheel,
    horizontalOdomWheel,
    DEAD_WHEEL_RADIUS
);

// Run odometry in a background task
void initialize() {
    odometry.start();
}

Usage​

Odometry provides the current heading and position of the robot on the field as a Pose object. The Pose provides x and y coordinates (in inches) representing the robot's position, as well as a heading (in degrees) representing the robot's orientation.

void opcontrol() {
    while (true) {
        // Get the current pose of the robot
        Pose pose = odometry.getPose();
        std::cout << "X: " << pose.x << " Y: " << pose.y << " Heading: " << pose.heading << std::endl;

        // Example: Resetting the robot's pose when a button is pressed
        bool resetPose = mainController.get_digital(DIGITAL_A);
        if (resetPose)
            odometry.setPose(Pose(0, 0, 0));

        // ...
    }
}

Configuration​

Inertial Sensor​

An Inertial Sensor (IMU) can be used to improve the accuracy of odometry calculations by providing accurate orientation data that drifts significantly less over time. While optional, it is highly recommended to use an IMU for the best performance.

// Define the Inertial Sensor
InertialSensor imu = InertialSensor("IMU", 15);

void initialize() {
    // Use the IMU for odometry calculations
    odometry.useIMU(&imu);

    // ...
}

Rotational Compensation​

When the robot rotates, the perpendicular orientation of the odometry wheels can inadvertently cause lateral movement readings. To correct for this, you can set the offsets of the odometry sensors relative to the robot's center of rotation.

These are defined as Vector2 objects, where the x-coordinate represents the horizontal offset (left/right) and the y-coordinate represents the vertical offset (forward/backward).

note

You may have to manually tune these values based on the position and grip of your odometry wheels.

// Offset values for the odometry sensors, relative to the robot's center of rotation (in inches)
Vector2 verticalSensorOffset = Vector2(-0.5, 0);    // (Y position is disregarded)
Vector2 horizontalSensorOffset = Vector2(0, 1);     // (X position is disregarded)

void initialize() {
    // Apply the offsets for odometry calculations
    odometry.setSensorOffsets(verticalSensorOffset, horizontalSensorOffset);

    // ...
}