OKapi throws the error:
9152 (OdomChassisController) ERROR: TwoEncoderOdometry: A tick diff (16711680) was greater than the maximum allowable diff (1000). Skipping this odometry step.
Someone said it could be a faulty encoder, but I checked and they output proper outputs.
Also, for some reason, it drives forward but then gets stuck after that.
void autonomous(){
std::shared_ptr<okapi::OdomChassisController> drive =
ChassisControllerBuilder()
.withMotors({-1,-2},{4,6})
// Green gearset, 4 in wheel diam, 11.5 in wheel track
.withDimensions({AbstractMotor::gearset::green, (84.0 / 60.0)},{{4_in, 12_in}, imev5GreenTPR})
.withSensors(
ADIEncoder{'A', 'B'},
ADIEncoder{'C', 'D'})
.withOdometry({{3.25_in, 12_in}, quadEncoderTPR}, StateMode::FRAME_TRANSFORMATION)
.buildOdometry();
drive->driveToPoint({2_ft, 0_ft});
pros::delay(1000);
printf( drive->getState().str().c_str());
std::cout << "step 1 \n";
drive->driveToPoint({2_ft, 2_ft});
pros::delay(1000);
printf( drive->getState().str().c_str());
std::cout << "step 2 \n";
drive->driveToPoint({0_ft, 2_ft});
pros::delay(1000);
printf( drive->getState().str().c_str());
std::cout << "step 3 \n";
drive->driveToPoint({0_ft, 0_ft});
pros::delay(1000);
printf( drive->getState().str().c_str());