After working on this autonomous P-Loop for the better part of a month, its just not working. This is the first year we’ve tried anything like this, so we have absolutely no prior knowledge on these things. Our loops values are all wonky, and after scouring it for a week we can’t find where our problem might come from. Currently, it works, but it’s not accurate whatsoever. We can’t change the values differently, like setting the left side target value to 400 and the right side target value to 800. Printing the different variables to the brain has only confused us more, because they all read the exact same value, which is I believe the right side rotation sensor value. Any help or input would be greatly appreciated.
This is all the relevant code I think, it is our P-Loop and the function we use to change the target values, by putting
drivePID(300, 300);
in our autonomous to make it drive forward 300 degrees.
int voltCap; //Global voltage cap
int targetLeft; //Global target value of the left drive motors
int targetRight; //Global target value of the right drive motors
int drivePIDFn() {
int errorLeft; //Error of left drive motors: target left value - actual left value
int errorRight; //Error of right drive motors: target right value - actual right value
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float kp = .01; //Proportional: multiplied by the error to determine the voltage rate
float kpTurn = .02; //Tune these if the robot is oscillating at the target value; if the robot does not stop after the movement is complete
int acc = 2; //Acceleration: how fast the voltage will increase over time; tune this value if robot is moving too quickly or slowly
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int voltageLeft = 0; //Variables used to tell the motors what voltage to run at
int voltageRight = 0; //Leave these at 0
int signLeft; //Used to determine if the error
int signRight; //is negative or positive
task::sleep(20);
while(Competition.isAutonomous()) { //Competition.isAutonomous() used to only run this function in the autonomous period
errorLeft = targetLeft - LRotationSens.position(degrees); //Error is the difference between the value you
errorRight = targetRight - RRotationSens.position(degrees); //want and the value the sensor is reading
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Brain.Screen.printAt(10,20, "pos L: %.2f ", LRotationSens.position(degrees) );
Brain.Screen.printAt(10,40, "pos R: %.2f ", RRotationSens.position(degrees) );
Brain.Screen.printAt(10,70, "Target Left: %.2f ", (targetLeft));
Brain.Screen.printAt(10,90, "Targer Right: %.2f ", (targetRight));
Brain.Screen.printAt(10,120, "Error Left: %.2f ", errorLeft);
Brain.Screen.printAt(10,140, "Error Right: %.2f ", errorRight);
Brain.Screen.printAt(10,170, "VoltageLeft: %.2f ", voltageLeft);
Brain.Screen.printAt(10,200, "VoltageRight: %.2f ", voltageRight);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
signLeft = (errorLeft > 0) - (errorLeft < 0); //Calculation for determining the error's sign; negative, positive, or 0
signRight = (errorRight > 0) - (errorRight < 0); //Left and right signs kept seperate to allow for turning
if (signLeft == signRight) {
voltageLeft = errorLeft * kp; //If the signs of the errors are equal, multiply error by the
voltageRight = errorRight * kp; //proportional constant to get the voltage of each side
} else if (signLeft != signRight){
voltageLeft = errorLeft * kpTurn; //If the signs of the errors are not equal, multiply error by the
voltageRight = errorRight * kpTurn; //turning proportional constant to get the voltage of each side
}
voltCap = voltCap + acc; //Uses the acceleration constant to increase the voltage cap each time the script runs
if (voltCap > 300) { //Caps the voltage
voltCap = 300;
}
if (abs(voltageLeft) > voltCap) { //Makes sure the voltage of the left side is in the correct direction and
voltageLeft = (voltCap * .04) * signLeft; //converts the voltage from 0<V<100 to -12<x<12 to give to the motors
}
if (abs(voltageRight) > voltCap) { //Makes sure the voltage of the right side is in the correct direction and
voltageRight = (voltCap * .04) * signRight; //converts the voltage from 0<V<100 to -12<x<12 to give to the motors
}
if (voltageRight > 7) {
voltageRight = 7;
} else if (voltageRight < -7){
voltageRight = -7;
}
if (voltageLeft > 7) {
voltageLeft = 7;
} else if (voltageLeft < -7) {
voltageLeft = -7;
}
FLDrive.spin(forward, voltageLeft, voltageUnits::volt); //Spins the motors based on the voltage for each side
BLDrive.spin(forward, voltageLeft, voltageUnits::volt);
FRDrive.spin(forward, voltageRight, voltageUnits::volt);
BRDrive.spin(forward, voltageRight, voltageUnits::volt);
task::sleep(20); //Keeps the function from not taking up too much brain processing power
}
return(1);
}
void drivePID(int left, int right) { //Main drive function to change target values in autonomous
targetLeft = targetLeft + left; //Used as drivePID(L, R); where L is how many degrees the left side needs
targetRight = targetRight + right; //to turn, and R being how many degrees the right side needs to turn
voltCap = 0; //Resets the voltcap for better movements when used multiple times
}