for some reason my robot will just spinning. The right side accelerates and then doesn’t stop when the left side does
here is the code.
int x;
int y;
int t;
// Constants for robot dimensions
const double WHEEL_RADIUS = 1.75; // Radius of the tracking wheels in inches
const double WHEEL_DISTANCE = 12.5; // Distance between the tracking wheels in inches
// Constants for PID control
const double KP = 0.3; // Proportional gain
const double KI = 0.05; // Integral gain
const double KD = 0.1; // Derivative gain
// Variables for odometry
double prevLeftEncoder = 0.0;
double prevRightEncoder = 0.0;
double xPos = 0.0;
double yPos = 0.0;
double theta = 0.0;
// Variables for PID control
double errorSum = 0.0;
double prevError = 0.0;
// Function to update the odometry
void updateOdometry() {
double leftEncoderSpot = leftEncoder.angle() * (2 * M_PI * WHEEL_RADIUS) / 360.0;
double rightEncoderSpot = rightEncoder.angle() * (2 * M_PI * WHEEL_RADIUS) / 360.0;
double deltaLeft = leftEncoderSpot - prevLeftEncoder;
double deltaRight = rightEncoderSpot - prevRightEncoder;
double deltaDistance = (deltaLeft + deltaRight) / 2.0;
double deltaTheta = (deltaRight - deltaLeft) / WHEEL_DISTANCE;
xPos += deltaDistance * cos(theta + deltaTheta / 2.0);
yPos += deltaDistance * sin(theta + deltaTheta / 2.0);
theta += deltaTheta;
prevLeftEncoder = leftEncoderSpot;
prevRightEncoder = rightEncoderSpot;
wait(20, msec);
}
// Function to perform PID control
double pidControl(double targetValue, double currentValue) {
double error = targetValue - currentValue;
errorSum += error;
double errorDiff = error - prevError;
double output = KP * error + KI * errorSum + KD * errorDiff;
prevError = error;
return output;
}
void stopAll(){
frontLeftMotor.stop();
frontRightMotor.stop();
backLeftMotor.stop();
backRightMotor.stop();
}
void odom(double x, double y, double t){
// Initialize the VEX Robotics system
vexcodeInit();
// Set the initial position and heading of the robot
xPos = 0.0;
yPos = 0.0;
theta = 0.0;
while (true) {
// Update odometry
updateOdometry();
// Perform PID control for a target position and heading
double targetX = x; // Target x-coordinate in inches
double targetY = y; // Target y-coordinate in inches
double targetTheta = t; // Target heading in degrees
double distanceError = sqrt(pow(targetX - xPos, 2) + pow(targetY - yPos, 2));
double headingError = targetTheta - theta;
double distanceOutput = pidControl(0.0, distanceError);
double thetaOutput = pidControl(0.0, headingError);
// Apply the PID output to the motors
double leftMotorSpeed = distanceOutput - thetaOutput;
double rightMotorSpeed = distanceOutput + thetaOutput;
// Set the motor speeds
frontLeftMotor.spin(forward, leftMotorSpeed, percent);
backLeftMotor.spin(forward, leftMotorSpeed, percent);
frontRightMotor.spin(forward, rightMotorSpeed, percent);
backRightMotor.spin(forward, rightMotorSpeed, percent);
// Delay for a short period of time
wait(20, msec);
updateOdometry();
}
}
this is what i am calling in auton
pidControl(5, 5);
odom(10,0, 0);
edit: code tags added by mods.