Whenever I run my RAMSETE path follower my bot starts sweeping left and right, while it does technically follow a similar path to the one planned it does the sweeping motion and never stops.
I am attempting to implement the RAMSETE path follower from the Purdue Sigbots Wiki: (RAMSETE Controller | Purdue SIGBots Wiki). I had to make a few changes because since I’m using squiggles for path planning which has the angles go positive as the bot turns left as well as the positive y direction to the left as the robot’s starting direction. Changes from the Purdue Sigbots Wiki include changing the rotation matrix to be clockwise, and switching which side of the drivetrain to add the angular velocity to. I know my odometry works. Any tips would be greatly appreciated. Below is the code:
/*---------------------------------------------------------------------------*/
/* Matrix Multiplication Function */
/* */
/* */
/*---------------------------------------------------------------------------*/
inline void multiplyMatrix(const std::array<std::array<double, 3>, 3> mat, const std::array<double, 3> vec, std::array<double, 3>& result) {
result[0] = mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2];
result[1] = mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2];
result[2] = mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2];
}
const double acceptableDistance = 2.0; // The distance the movement controller will continue until
/*---------------------------------------------------------------------------*/
/* RAMSETE Point Follower Function */
/* */
/* */
/*---------------------------------------------------------------------------*/
const double b = 2.0; // Gain for controller. Must be >= 1. Greater number leads to a more aggressive controller
const double d = 0.7; // Dampening for controller. Must be 0 <= d <= 1. Smaller value of d leads to more dampening
//const double motorToLinearSpeedRatio = (36.0 / 60.0) * 26.0; // Found by multiplying the ratio of the geared drivetrain by the circumfrence of the wheels in cm
void moveToPointRAMSETE(RAMSETEPoint point){
// Calculate the overall gain constant (k) for this point
double k = 2.0 * d * std::sqrt(std::pow(point.desiredAngularVel, 2.0) + b * std::pow(point.desiredLinearVel, 2.0));
double desiredLinearVel = point.desiredLinearVel;
double desiredAngularVel = point.desiredAngularVel;
while (currDistToPoint(point) >= acceptableDistance){
// Define a 3x3 matrix with the needed values
// Creates the following matrix:
// [ cos(currentHeading) sin(currentHeading) 0]
// [-sin(currentHeading) cos(currentHeading) 0]
// [ 0 0 1]
std::array<std::array<double, 3>, 3> mat = {{
{std::cos(globalHeading), -std::sin(globalHeading), 0.0},
{std::sin(globalHeading), std::cos(globalHeading), 0.0},
{0.0, 0.0, 1.0}
}};
double tempHeadingError = point.heading - globalHeading;
if (tempHeadingError > M_PI){
tempHeadingError -= M_TWOPI;
}else if (tempHeadingError < -M_PI){
tempHeadingError += M_TWOPI;
}
// Define a 3x1 vector containing the raw linear and heading errors (Not taking into account additional aspects)
std::array<double, 3> vec = {point.xPos - globalXPos, point.yPos - globalYPos, tempHeadingError};
// To store the resulting errors
std::array<double, 3> computedErrors;
// Perform matrix multiplication
// This allows us to calculate the linear and heading errors while taking into account the current orientation of the robot
multiplyMatrix(mat, vec, computedErrors);
// Pull out the individual errors
double xError = computedErrors[0];
double yError = computedErrors[1];
double headingError = computedErrors[2];
// If this is the last point, we should adjust the desired velocity to roughly scale to a very small value as we reach the point
// This allows the robot to stop at the point
if (point.stopAtPoint){
// Calculate desired velocities that approach 0 as we reach the point
desiredLinearVel = 0.01 * xError;
desiredAngularVel = 0.01 * headingError;
// Calculate the new gain constant
k = 2.0 * d * std::sqrt(std::pow(desiredAngularVel, 2.0) + b * std::pow(desiredLinearVel, 2.0));
}
// Calculate the required linear and angular velocities
double v = desiredLinearVel * std::cos(headingError) + k * xError;
double w = desiredAngularVel + k * headingError + (b * desiredLinearVel * std::sin(headingError) * yError) / headingError;
// Calculating the speed the motor needs to spin at to generate the required linear velocity and angular velocity
double vMotor = (v / 26.0) * 60.0 * (60.0 / 36.0); // Drivetrain is geared 60 on wheels to 36 on motors, wheel diameter is 26 cm
double wMotor = w * SR * (60.0 / 36.0) * 60.0; // Drivetrain is geared 60 on wheels to 36 on motors
// Calculate the speeds the left and right sides of a tank drivetrain need to spin at to achieve the calculated linear and angular velocities
double leftVel = vMotor - wMotor;
double rightVel = vMotor + wMotor;
// Brain.Screen.clearScreen();
// Brain.Screen.setCursor(1, 1);
// Brain.Screen.print("vMotor Val: ");
// Brain.Screen.print(vMotor);
// Brain.Screen.setCursor(2, 1);
// Brain.Screen.print("V Val: ");
// Brain.Screen.print(v);
// Brain.Screen.setCursor(3, 1);
// Brain.Screen.print("W Val: ");
// Brain.Screen.print(w);
Front_Left.setVelocity(leftVel, rpm);
Bottom_Left.setVelocity(leftVel, rpm);
Top_Left.setVelocity(leftVel, rpm);
Front_Right.setVelocity(rightVel, rpm);
Bottom_Right.setVelocity(rightVel, rpm);
Top_Right.setVelocity(rightVel, rpm);
// Spin each motor
Front_Left.spin(vex::forward);
Front_Right.spin(vex::forward);
Bottom_Left.spin(vex::forward);
Bottom_Right.spin(vex::forward);
Top_Left.spin(vex::forward);
Top_Right.spin(vex::forward);
wait(10, msec);
}
}