Hey everyone, the code below doesn’t work as I want it to. Essentially, the last setState has 0 effect on the robot and the robot doesn’t move backward 1 foot; instead, it turns 90 degrees and then moves backward 1 foot. When I log the state, I get a ThreeEncoderOdometry Error (a tick diff was greater than the maximum allowable diff). Presumably, this would mean the encoders are bad, but if they were bad, none of the previous movements would work either. Thanks for any help.
std::shared_ptr<OdomChassisController> odomchas =
ChassisControllerBuilder()
.withMotors(DRIVE_FRONT_LEFT, DRIVE_FRONT_RIGHT, DRIVE_BACK_RIGHT, DRIVE_BACK_LEFT)
//.withSensors(leftencoder, rightencoder, middleencoder)
.withGains(
{0.002, 0.00001, 0}, // Distance controller gains
{0.007, 0, 0}, // Turn controller gains
{0.002, 0, 0.00006} // Angle controller gains (helps drive straight)
)
.withSensors(
ADIEncoder{'G', 'H'},
ADIEncoder{'C', 'D', true},
ADIEncoder{'E', 'F'}
)
.withDimensions(AbstractMotor::gearset::green, {{2.75_in, 7_in, 1_in, 2.75_in}, quadEncoderTPR})
.withOdometry()
.buildOdometry();
void autonomous() {
odomchas->setState({0_in,0_in,0_deg});
// //initalize
xModel->strafe(-50);
pros::delay(600);
xModel->strafe(0);
//intake deploy
left_intake.move_velocity(-100);
right_intake.move_velocity(-100);
indexer.move_velocity(-200);
main_intake.move_velocity(200);
odomchas->turnToAngle(-90_deg);
//
// //bottom right corner
left_intake.move_velocity(200);
right_intake.move_velocity(200);
odomchas->turnToAngle(-90_deg);
main_intake.move_velocity(200);
indexer.move_velocity(-200);
odomchas->setState({0_in,0_in,0_deg});
odomchas->driveToPoint({2.25_ft, 0_ft});
pros::delay(500);
left_intake.move_velocity(0);
right_intake.move_velocity(0);
indexer.move_velocity(0);
main_intake.move_velocity(0);
odomchas->setState({0_in,0_in,0_deg});
odomchas->driveToPoint({1_ft, 0_ft}, true);
}