Currently I am working with Odometry and a PID, which work great going forward. It goes the distance I set it to, but then we I set it to turn, it turns for infinity and can’t seem to recognize what the problem is.
I am using one odom wheel for sideways movement, and then the internal encoders for the forward movement. I put the code down below.
CustomOdomerty.cpp
std::valarray<double> CustomOdometry::getSensorVals() // returns new sensor values
{
return {meS.get(), (def::mtr_dt_right_front.getPosition())* def::DRIVE_DEG_TO_IN,
((isinf(mimu1.get_rotation()) ? 0 : mimu1.get_rotation()) +
(isinf(mimu2.get_rotation()) ? 0 : mimu2.get_rotation())) *
M_PI / -360};
}
Drivetrain.cpp
void Drivetrain::turnToAngle(QAngle iangle, std::vector<AsyncAction> iactions, PID ipid)
{
enable(); // make sure the action can run
while (menabled && !ipid.isSettled())
{
double angleError = util::wrapDeg180((iangle - Drivetrain::getTheta()).convert(degree)); // calculates the angle the robot needs to turn
Drivetrain::moveArcade(0, -ipid.iterate(angleError), false);
// std::cout << "in angle loop. Error: " << angleError << " use: " << counter << std::endl;
pros::delay(20);
}
Drivetrain::moveArcade(0, 0, false);
def::controller.rumble("-");
}