General

Tracking Wheels

odom_tracker_left_set()

Sets the parallel left tracking wheel for odometry.

input an ez::tracking_wheel

Prototype

void odom_tracker_left_set(tracking_wheel* input);

Example

ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  chassis.odom_tracker_right_set(&right_tracker);
  chassis.odom_tracker_left_set(&left_tracker);
  chassis.odom_tracker_back_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
}

odom_tracker_right_set()

Sets the parallel right tracking wheel for odometry.

input an ez::tracking_wheel

Prototype

void odom_tracker_right_set(tracking_wheel* input);

Example

ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  chassis.odom_tracker_right_set(&right_tracker);
  chassis.odom_tracker_left_set(&left_tracker);
  chassis.odom_tracker_back_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
}

odom_tracker_front_set()

Sets the perpendicular front tracking wheel for odometry.

input an ez::tracking_wheel

Prototype

ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  chassis.odom_tracker_right_set(&right_tracker);
  chassis.odom_tracker_left_set(&left_tracker);
  chassis.odom_tracker_front_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
}

Example

void odom_tracker_front_set(tracking_wheel* input);

odom_tracker_back_set()

Sets the perpendicular back tracking wheel for odometry.

input an ez::tracking_wheel

Prototype

void odom_tracker_back_set(tracking_wheel* input);

Example

ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  chassis.odom_tracker_right_set(&right_tracker);
  chassis.odom_tracker_left_set(&left_tracker);
  chassis.odom_tracker_back_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
}

Pose

odom_enable()

Enables / disables tracking.

input true enables tracking, false disables tracking

Prototype

void odom_enable(bool input);

Example

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  // Enable tracking
  chassis.odom_enable(true);
}

odom_enabled()

Returns whether the bot is tracking with odometry.

True means tracking is enabled, false means tracking is disabled.

Prototype

bool odom_enabled();

Example

void initialize() {
  // Print our branding over your terminal :D
  ez::ez_template_print();

  pros::delay(500);  // Stop the user from doing anything while legacy ports configure

  // Enable tracking
  chassis.odom_enable(true);

  if (chassis.odom_enabled()) 
    printf("Odom is enabled!\n");
}

odom_x_set()

Sets the current x position of the robot.

x new x coordinate in inches

Prototype

void odom_x_set(double x);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 0_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_x_set(0);  // Set current x to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, 0_in}, rev, 110});
  chassis.pid_wait();
}

odom_x_set()

Sets the current X coordinate of the robot.

p_x new x coordinate as an okapi unit

Prototype

void odom_x_set(okapi::QLength p_x);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 0_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_x_set(0_in);  // Set current x to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, 0_in}, rev, 110});
  chassis.pid_wait();
}

odom_y_set()

Sets the current Y coordinate of the robot.

y new y coordinate in inches

Prototype

void odom_y_set(double y);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{0_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_y_set(0);  // Set current y to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{0_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_y_set()

Sets the current Y coordinate of the robot.

p_y new y coordinate as an okapi unit

Prototype

void odom_y_set(okapi::QLength p_y);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{0_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_y_set(0_in);  // Set current y to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{0_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_theta_set()

Sets the current Theta of the robot.

a new angle in degrees

Prototype

void odom_theta_set(double a);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_turn_set(90_deg, 110);
  chassis.pid_wait();

  chassis.odom_theta_set(0);  // Set current theta to 0

  // This will go back to the starting location
  chassis.pid_turn_set(-90_deg, 110);
  chassis.pid_wait();
}

odom_theta_set()

Sets the current angle of the robot.

p_y new angle as an okapi unit

Prototype

void odom_theta_set(okapi::QAngle p_a);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_turn_set(90_deg, 110);
  chassis.pid_wait();

  chassis.odom_theta_set(0_deg);  // Set current theta to 0

  // This will go back to the starting location
  chassis.pid_turn_set(-90_deg, 110);
  chassis.pid_wait();
}

odom_xy_set()

Sets the current X and Y coordinate for the robot.

x new x value, in inches
y new y value, in inches

Prototype

void odom_xy_set(double x, double y);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_xy_set(0, 0);  // Set current x and y to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_xy_set()

Sets the current X and Y coordinate for the robot.

p_x new x value, okapi unit
p_y new y value, okapi unit

Prototype

void odom_xy_set(okapi::QLength p_x, okapi::QLength p_y);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_xy_set(0_in, 0_in);  // Set current x and y to 0

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_xyt_set()

Sets the current X, Y, and Theta values for the robot.

x new x value, in inches
y new y value, in inches
t new theta value, in degrees

Prototype

void odom_xyt_set(double x, double y, double t);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_xyt_set(0, 0, -45);  

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_xy_set()

Sets the current X, Y, and Theta values for the robot.

p_x new x value, okapi unit
p_y new y value, okapi unit
p_t new theta value, okapi unit

Prototype

void odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  chassis.odom_xyt_set(0_in, 0_in, -45_deg);  

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_pose_set()

Sets the current pose of the robot.

itarget {x, y, t} units in inches and degrees

Prototype

void odom_pose_set(pose itarget);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  ez::pose new_pose = {0, 0, -45};
  chassis.odom_xyt_set(new_pose);  

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_pose_set()

Set the current pose of the robot.

itarget {x, y, t} as an okapi unit

Prototype

void odom_pose_set(united_pose itarget);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  ez::united_pose new_pose = {0_in, 0_in, -45_deg};
  chassis.odom_xyt_set(new_pose);  

  // This will go back to the starting location
  chassis.pid_odom_set({{-24_in, -24_in}, rev, 110});
  chassis.pid_wait();
}

odom_x_flip()

Flips the C axis.

flip true means left is positive x, false means right is positive x

Prototype

void odom_x_flip(bool flip = true);

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_x_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_x_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

odom_y_flip()

Flips the Y axis.

flip true means down is positive y, false means up is positive y

Prototype

void odom_y_flip(bool flip = true);

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_y_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_y_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 90_deg);   // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

odom_theta_flip()

Flips the rotation axis. This works for odom and non odom functions.

flip true means counterclockwise is positive, false means clockwise is positive

Prototype

void odom_theta_flip(bool flip = true);

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_theta_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_x_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

odom_x_get()

Returns the current x position of the robot in inches.

Prototype

double odom_x_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  printf("X: %.2f  Y: %.2f  T: %.2f\n", chassis.odom_x_get(), chassis.odom_y_get(), chassis.odom_theta_get());
}

odom_y_get()

Returns the current Y coordinate of the robot in inches.

Prototype

double odom_y_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  printf("X: %.2f  Y: %.2f  T: %.2f\n", chassis.odom_x_get(), chassis.odom_y_get(), chassis.odom_theta_get());
}

odom_theta_get()

Returns the current Theta of the robot in degrees.

Prototype

double odom_theta_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  printf("X: %.2f  Y: %.2f  T: %.2f\n", chassis.odom_x_get(), chassis.odom_y_get(), chassis.odom_theta_get());
}

odom_pose_get()

Returns the current pose of the robot.

Prototype

pose odom_pose_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_set({{24_in, 24_in, 45_deg}, fwd, 110});
  chassis.pid_wait();

  ez::pose c_pose = chassis.odom_pose_get();
  printf("X: %.2f  Y: %.2f  T: %.2f\n", c_pose.x, c_pose.y, c_pose.theta);
}

odom_x_direction_get()

Checks if X axis is flipped.

True means left is positive X, false means right is positive X.

Prototype

bool odom_x_direction_get();

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_x_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_x_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

odom_y_direction_get()

Checks if Y axis is flipped.

True means down is positive Y, false means up is positive Y.

Prototype

bool odom_y_direction_get();

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_y_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_y_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 90_deg);   // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

odom_theta_direction_get()

Checks if the rotation axis is flipped.

True means counterclockwise is positive, false means clockwise is positive.

Prototype

bool odom_theta_direction_get();

Example

void base() {
  ez::united_pose start_intaking_here;

  if (chassis.odom_theta_direction_get() == false)
    start_intaking_here = {12_in, 24_in};  // If red
  else
    start_intaking_here = {15_in, 24_in};  // If blue

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {start_intaking_here, rev, 110},
                        {{24_in, 24_in}, rev, 110}},
                       true);
  chassis.pid_wait_until_index(1);  // Waits until the robot passes 12, 24
  Intake.move(127);
  chassis.pid_wait();

  chassis.pid_odom_set({{24_in, 48_in, 45_deg}, fwd, 110},
                       true);
  chassis.pid_wait();

  chassis.pid_turn_set(90_deg, 110, true);
  chassis.pid_wait();
}

void red() {
  base();
}

void blue() {
  chassis.odom_x_flip();
  chassis.odom_theta_flip();
  base();
}


void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  red(); 
  // blue();
}

Movement Constants

pid_odom_angular_constants_set()

Set the odom angular pid constants object.

p proportional term
i integral term
d derivative term
p_start_i error threshold to start integral

Prototype

void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);

Example

void initialize() {
  chassis.pid_odom_angular_constants_set(5, 0, 50);
}

odom_turn_bias_set()

A proportion of how prioritized turning is during odometry motions.

Turning is prioritized so the robot "applies brakes" while turning. Lower number means more braking.

bias a number between 0 and 1

Prototype

void odom_turn_bias_set(double bias);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_turn_bias_set(1.0);  // Set turn bias to 1

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  // Reset your angle and position
  chassis.pid_turn_set(0_deg, 110);
  chassis.pid_wait();
  chassis.odom_xyt_set(0_in, 0_in);

  chassis.odom_turn_bias_set(0.5);  // Set turn bias to 0

  // Go to 24, 24 relative to where the robot ended, but with a new turn bias
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

pid_odom_drive_exit_condition_set()

Set's constants for odom driving exit conditions.

p_small_exit_time time to exit when within small_error, okapi unit
p_small_error small timer will start when error is within this, okapi unit
p_big_exit_time time to exit when within big_error, okapi unit
p_big_error big timer will start when error is within this, okapi unit
p_velocity_exit_time time, in okapi units, for velocity to be 0
p_mA_timeout velocity timer will start when velocity is 0, okapi unit
use_imu true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't

Prototype

void pid_odom_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu = true);

Example

void initialize() {
  chassis.pid_odom_drive_exit_condition_set(300_ms, 1_in, 500_ms, 3_in, 750_ms, 750_ms);
}

pid_odom_drive_exit_condition_set()

Set's constants for odom driving exit conditions.

p_small_exit_time time to exit when within small_error, in ms
p_small_error small timer will start when error is within this, in inches
p_big_exit_time time to exit when within big_error, in ms
p_big_error big timer will start when error is within this, in inches
p_velocity_exit_time velocity timer will start when velocity is 0, in ms
p_mA_timeout mA timer will start when the motors are pulling too much current, in ms
use_imu true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't

Prototype

void pid_odom_drive_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu = true);

Example

void initialize() {
  chassis.pid_odom_drive_exit_condition_set(300, 1, 500, 3, 750, 750);
}

pid_odom_turn_exit_condition_set()

Set's constants for odom turning exit conditions.

p_small_exit_time time to exit when within small_error, okapi unit
p_small_error small timer will start when error is within this, okapi unit
p_big_exit_time time to exit when within big_error, okapi unit
p_big_error big timer will start when error is within this, okapi unit
p_velocity_exit_time time, in okapi units, for velocity to be 0
p_mA_timeout velocity timer will start when velocity is 0, okapi unit
use_imu true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't

Prototype

void pid_odom_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu = true);

Example

void initialize() {
  chassis.pid_odom_turn_exit_condition_set(300_ms, 3_deg, 500_ms, 3_deg, 750_ms, 750_ms);
}

pid_odom_turn_exit_condition_set()

Set's constants for odom turning exit conditions.

p_small_exit_time time to exit when within small_error, in ms
p_small_error small timer will start when error is within this, in degrees p_big_exit_time time to exit when within big_error, in ms p_big_error big timer will start when error is within this, in degrees p_velocity_exit_time velocity timer will start when velocity is 0, in ms p_mA_timeout mA timer will start when the motors are pulling too much current, in ms
use_imu true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't

Prototype

void pid_odom_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu = true);

Example

void initialize() {
  chassis.pid_odom_turn_exit_condition_set(300, 3, 500, 7, 750, 750);
}

odom_look_ahead_set()

Sets how far away the robot looks in the path during pure pursuits.

distance how long the "carrot on a stick" is, in inches

Prototype

void odom_look_ahead_set(double distance);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_look_ahead_set(7.0);  // Set look ahead to 7in

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  // Reset your angle and position
  chassis.pid_turn_set(0_deg, 110);
  chassis.pid_wait();
  chassis.odom_xyt_set(0_in, 0_in);

  chassis.odom_look_ahead_set(14.0);  // Set look ahead to 14in

  // Go to 24, 24 relative to where the robot ended, but with a new look ahead
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

odom_look_ahead_set()

Sets how far away the robot looks in the path during pure pursuits.

distance how long the "carrot on a stick" is, in okapi units

Prototype

void odom_look_ahead_set(okapi::QLength p_distance);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_look_ahead_set(7_in);  // Set look ahead to 7in

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  // Reset your angle and position
  chassis.pid_turn_set(0_deg, 110);
  chassis.pid_wait();
  chassis.odom_xyt_set(0_in, 0_in);

  chassis.odom_look_ahead_set(14_in);  // Set look ahead to 14in

  // Go to 24, 24 relative to where the robot ended, but with a new look ahead
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

pid_odom_behavior_set()

Sets the default behavior for turns in odom turning movements.

behavior ez::shortest, ez::longest, ez::left, ez::right, ez::raw

Prototype

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_behavior_set(ez::longest);  // Set the robot to take the longest path there

  // This will make the robot go the long way around to get to 24, 0
  chassis.pid_odom_set({{24_in, 0_in}, fwd, 110});
  chassis.pid_wait();
}

Example

void pid_odom_behavior_set(e_angle_behavior behavior);

pid_odom_behavior_get()

Returns the turn behavior for odom turns.

Prototype

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_odom_behavior_set(ez::longest);  // Set the robot to take the longest path there

  if (chassis.pid_odom_behavior_get() == ez::longest) {
    printf("Current behavior is ez::longest!\n");
  } else {
    printf("Current behavior is not ez::longest!\n");
  }

  // This will make the robot go the long way around to get to 24, 0
  chassis.pid_odom_set({{24_in, 0_in}, fwd, 110});
  chassis.pid_wait();
}

Example

e_angle_behavior pid_odom_behavior_get();

odom_path_spacing_set()

Sets the spacing between points when points get injected into the path.

p_spacing a small number in okapi units

Prototype

void odom_path_spacing_set(okapi::QLength p_spacing);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_path_spacing_set(0.5_in);  // Set path spacing to 0.5 inches
  printf("Path Spacing: %.2f\n", chassis.odom_path_spacing_get());

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

odom_path_spacing_set()

Sets the spacing between points when points get injected into the path.

spacing a small number in inches

Prototype

void odom_path_spacing_set(double spacing);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_path_spacing_set(0.5_in);  // Set path spacing to 0.5 inches
  printf("Path Spacing: %.2f\n", chassis.odom_path_spacing_get());

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

odom_turn_bias_get()

Returns the proportion of how prioritized turning is during odometry motions.

Prototype

double odom_turn_bias_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_turn_bias_set(1.0);  // Set turn bias to 1
  printf("Turn Bias: %.2f\n", chassis.odom_turn_bias_get());

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  // Reset your angle and position
  chassis.pid_turn_set(0_deg, 110);
  chassis.pid_wait();
  chassis.odom_xyt_set(0_in, 0_in);

  chassis.odom_turn_bias_set(0.5);  // Set turn bias to 0
  printf("Turn Bias: %.2f\n", chassis.odom_turn_bias_get());

  // Go to 24, 24 relative to where the robot ended, but with a new turn bias
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

odom_look_ahead_get()

Returns how far away the robot looks in the path during pure pursuits.

Prototype

double odom_look_ahead_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_look_ahead_set(7_in);  // Set look ahead to 7in
  printf("Look Ahead: %.2f\n", chassis.odom_look_ahead_get());

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();

  // Reset your angle and position
  chassis.pid_turn_set(0_deg, 110);
  chassis.pid_wait();
  chassis.odom_xyt_set(0_in, 0_in);

  chassis.odom_look_ahead_set(14_in);  // Set look ahead to 14in
  printf("Look Ahead: %.2f\n", chassis.odom_look_ahead_get());

  // Go to 24, 24 relative to where the robot ended, but with a new look ahead
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

odom_path_spacing_get()

Returns the spacing between points when points get injected into the path.

Prototype

double odom_path_spacing_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.odom_path_spacing_set(0.5_in);  // Set path spacing to 0.5 inches
  printf("Path Spacing: %.2f\n", chassis.odom_path_spacing_get());

  // Go to 24, 24
  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110});
  chassis.pid_wait();
}

slew_odom_reenable()

Allows slew to reenable when the new input speed is larger than the current speed during pure pursuits.

slew_on true enables, false disables

Prototype

void slew_odom_reenable(bool reenable);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  // Allow slew to reenable when speed dips too low
  chassis.slew_odom_reenable(true);

  // This will slew from the start to 110, 
  // and once the speed dips down to 50 it'll slew again up to 90
  chassis.pid_odom_set({{{6_in, 10_in}, fwd, 110},
                        {{0_in, 20_in}, fwd, 50},
                        {{0_in, 30_in}, fwd, 90}},
                       true);
  chassis.pid_wait();
}

slew_odom_reenabled()

Allows slew to reenable when the new input speed is larger than the current speed during pure pursuits.

Prototype

bool slew_odom_reenabled();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  // Allow slew to reenable when speed dips too low
  chassis.slew_odom_reenable(true);
  if (chassis.slew_odom_reenabled()) 
    printf("Slew will Reenable in Odom!\n");

  // This will slew from the start to 110, 
  // and once the speed dips down to 50 it'll slew again up to 90
  chassis.pid_odom_set({{{6_in, 10_in}, fwd, 110},
                        {{0_in, 20_in}, fwd, 50},
                        {{0_in, 30_in}, fwd, 90}},
                       true);
  chassis.pid_wait();
}

odom_path_smooth_constants_set()

Sets the constants for smoothing out a path.

Path smoothing based on https://medium.com/@jaems33/understanding-robot-motion-path-smoothing-5970c8363bc4

weight_smooth how much weight to update the data
weight_data how much weight to smooth the coordinates
tolerance how much change per iteration is necessary to keep iterating

Prototype

void odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance);

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_drive_toggle(false);  // Disable the drive
  chassis.pid_print_toggle(false);  // Disable movement printing

  // Print default smooth constants
  std::vector<double> smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal

  // Print updated smooth constants
  chassis.odom_path_smooth_constants_set(0.5, 0.003, 0.0001);
  smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal
}

odom_path_smooth_constants_get()

Returns the constants for smoothing out a path.

In order of:

• weight_smooth
• weight_data
• tolerance

Prototype

std::vector<double> odom_path_smooth_constants_get();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_drive_toggle(false);  // Disable the drive
  chassis.pid_print_toggle(false);  // Disable movement printing

  // Print default smooth constants
  std::vector<double> smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal

  // Print updated smooth constants
  chassis.odom_path_smooth_constants_set(0.5, 0.003, 0.0001);
  smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal
}

odom_path_print()

Prints the current path the robot is following.

Prototype

void odom_path_print();

Example

void autonomous() {
  chassis.pid_targets_reset();                // Resets PID targets to 0
  chassis.drive_imu_reset();                  // Reset gyro position to 0
  chassis.drive_sensor_reset();               // Reset drive sensors to 0
  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency

  chassis.pid_drive_toggle(false);  // Disable the drive
  chassis.pid_print_toggle(false);  // Disable movement printing

  // Print default smooth constants
  std::vector<double> smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal

  // Print updated smooth constants
  chassis.odom_path_smooth_constants_set(0.5, 0.003, 0.0001);
  smooth_consts = chassis.odom_path_smooth_constants_get();
  printf("Weight Smooth: %.2f   Weight Data: %.2f   Tolerance: %.2f\n", smooth_consts[0], smooth_consts[1], smooth_consts[2]);

  chassis.pid_odom_set({{{0_in, 24_in}, fwd, 110},
                        {{24_in, 24_in}, fwd, 110}},
                       false);
  chassis.odom_path_print();  // Print the full path to terminal
}

Boomerang Behavior

pid_odom_boomerang_constants_set()

Set the odom boomerang pid constants object.

p proportional term
i integral term
d derivative term
p_start_i error threshold to start integral

Prototype

void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);

Example

void initialize() {
  chassis.pid_odom_boomerang_constants_set(5, 0, 50);
}

odom_boomerang_dlead_set()

Sets a new dlead.

Dlead is a proportional value of how much to make the robot curve during boomerang motions.

input a value between 0 and 1

Prototype

void odom_boomerang_dlead_set(double input);

Example

void initialize() {
  chassis.odom_boomerang_dlead_set(0.5);
}

odom_boomerang_distance_set()

Sets how far away the carrot point can be from the target point.

distance distance in inches

Prototype

void odom_boomerang_distance_set(double distance);

Example

void initialize() {
  chassis.odom_boomerang_dlead_set(0.5);
  chassis.odom_boomerang_distance_set(12);
}

odom_boomerang_distance_set()

Sets how far away the carrot point can be from the target point.

distance distance as an okapi unit

Prototype

void odom_boomerang_distance_set(okapi::QLength p_distance);

Example

void initialize() {
  chassis.odom_boomerang_dlead_set(0.5);
  chassis.odom_boomerang_distance_set(12_in);
}

odom_boomerang_dlead_get()

Returns the current dlead.

Prototype

double odom_boomerang_dlead_get();

Example

void initialize() {
  chassis.odom_boomerang_dlead_set(0.5);

  printf("dlead: %.2f\n", chassis.odom_boomerang_dlead_get());
}

odom_boomerang_distance_get()

Returns the maximum distance the carrot point can be away from the target point.

Prototype

double odom_boomerang_distance_get();

Example

void initialize() {
  chassis.odom_boomerang_dlead_set(0.5);
  chassis.odom_boomerang_distance_set(12_in);

  printf("Max Distance: %.2f\n", chassis.odom_boomerang_distance_get());
}