Help with odom

Programming Support
1

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.

2

I would troubleshoot each part of the system individually. First, print your odom output values (x coord, y coord, and heading) to the screen and move the bot around to make sure the odom works. Then test out your pid function with a simple (non-odom) movement. Then make sure your systems work together.

With that said, the one problem that I see is error = desiredheading-currentheading (or whatever the variable names are). That’ll mess you up if you want to go to a heading of 0 and your current heading is 270. I’d make a function that picks the right direction and error based on the desired heading and current heading.

1 Like
3

Firstly the first line of your auton does absolutely nothing since error is zero. Even if the error wasnt zero all it would do is return a double and nothing else.

Second, you are updating odom once at the top of the loop and once at the bottom. I think it would be better to be consistent.

Third, verify your functions are working correctly by printing out values. Your description of the issue is vague and numerical data would help. Verify that your odom is working independently of the drive to the point then verify that the error and PID outputs are correct.

Also, the while(true) loops will never exit so maybe that’s a potential issue.

1 Like