Pid with odometry keeps arcing

Programming Support VEXcode Pro V5 Text Tech Support
,
1

So I’m trying to use pid and odometry together and everytime the robot is instructed to go straight, it arcs to the left. It might e because of the setup of the wheels and i have to add a filter but I just want to make sure my code is also good.

Sorry for lack of commenting and messy code

int odometry(){
  
  while(true){
    currentParallel=parallelSensor.position(deg)*(M_PI*2.75)/360;
    currentPerp=perpSensor.position(deg)*(M_PI*2.75)/360.0;

    deltaParallel=currentParallel-previousParallel;
    deltaPerp=currentPerp-previousPerp;

    previousParallel=currentParallel;
    previousPerp=currentPerp;
    totalParallel+=deltaParallel;

    currentTheta=Inertial.rotation()*M_PI/180.0;
    deltaTheta=currentTheta-previousTheta;
    previousTheta=currentTheta;

    if (deltaTheta<1){
      deltaXLocal=deltaPerp;
      deltaYLocal=deltaParallel;
    }
    else{
      deltaXLocal=2*sin(deltaTheta/2.0)*((deltaPerp/deltaTheta)+dB);
      deltaYLocal=2*sin(deltaTheta/2.0)*(deltaParallel/deltaTheta);
    }

    averageTheta=currentTheta-(deltaTheta/2.0);

    deltaXGlobal=(deltaYLocal*cos(averageTheta))-(deltaXLocal*sin(averageTheta));
    deltaYGlobal=(deltaYLocal*sin(averageTheta))+(deltaXLocal*cos(averageTheta));

    currentX+=deltaXGlobal;
    currentY+=deltaYGlobal;

    wait(10,msec);
  }
  return 1;
}
task Odometry(odometry);


void drivePD(double xFinalValue, double yFinalValue, double turnFinalValue){
  double drivekP=.2567;
  double drivekD=.0085;
  double xError=xFinalValue;
  double yError=yFinalValue;
  double turnError=turnFinalValue;
  double xPreviousError=0;
  double yPreviousError=0;
  double turnPreviousError=0;
  while (true){
    xError=xFinalValue-currentX;
    yError=yFinalValue-currentY;
    turnError=turnFinalValue-Inertial.rotation();

    double xDerivative=xError-xPreviousError;
    double yDerivative=yError-yPreviousError;
    double turnDerivative=turnError-turnPreviousError;

    double yMotorPower=yError*drivekP+yDerivative*drivekD;
    double xMotorPower=xError*drivekP+xDerivative*drivekD;
    double turnMotorPower=turnError*drivekP+turnDerivative*drivekD;

    rightSide.spin(fwd,yMotorPower-xMotorPower-turnMotorPower,volt);
    leftSide.spin(fwd,yMotorPower+xMotorPower+turnMotorPower,volt);

    
    xPreviousError=xError;
    yPreviousError=yError;
    turnPreviousError=turnError;

    wait(10,msec);
    Brain.Screen.printAt(20,20,"%f",xError);
    Brain.Screen.printAt(20,40,"%f",currentX);
  }
  driveTrain.stop(brake);
}
2

Could you explain your variables and robot configuration?

3

so our robot is using 2 tracking wheels and one inertial sensor due to space constraints. the the perpindicular sensor is called perp and the parallel sensor is called parallelSensor. I used a lot of global variables but current is the updated position, previous is the position from previous iteration, delta is the change in the iterations. the total is obviously the total change in position from the start. the two if statements are for if there was a neglibible change in angle or not and then the local, global, and current x and y are for the coordinates, with the current x and y being the ones used for pid calculations.i mostly used 5225a documentation to implement all of this.

in pid, i have three desired values in the x, y, and turn values and each go through their own pd calculations to get their own motor powers, which are added or subtracted in the end for final motor power.

4

your x and y values are also related to turning, assuming your odometry is correct. Your driving should be based on relative position. Have you try xFinalValue and turnFinalValue zero?

5

yes, i’m testing with the x and turn values at 0 and it is still arcing left. Do you think it’s could be related to the perpindicular wheel turning when it’s not supposed to?

6

I would test for the mechanic first. Do your left and right wheels balance?

Feed the same volt to both wheels and see if it goes straight.
Then print x value, error, left and right volt you feed to motors for debugging.