After switching from Okapilib’s chassis controller integrated to chassis controller PID (By declaring withgains), the robot can no longer even drive straight, moving forward slightly before veering sharply off in a swing turn infinitely (anti-clockwise, or counter-clockwise for you Americans) when the moveDistance function is called. The PID gains aren’t tuned well yet though so don’t judge the code too much, its very much a test. The odometry reads the correct position after a translation, the same code was used but with .withgains commented and it worked fine (integrated PID controller). Here is the code:
#include "main.h"
#include "okapi/api.hpp"
using namespace okapi;
std::shared_ptr<OdomChassisController> chassis = ChassisControllerBuilder()
.withMotors({1, -2}, {-4, 3})
.withDimensions(AbstractMotor::gearset::green, {{3.25_in, 393_mm}, 540})
.withSensors
(
ADIEncoder{'A', 'B'},
ADIEncoder{'E', 'F'},
ADIEncoder{'C', 'D', true}
)
.withGains({0.002, 0, 0.00003}, {0.001, 0, 0.00003}, {0.001, 0, 0.00003})
//.withClosedLoopControllerTimeUtil(50, 5, 200_ms)
.withOdometry({{2.75_in, 212_mm, 120_mm, 2.75_in}, quadEncoderTPR})
.buildOdometry();