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);
}