Autonomous P-Loop not working at all

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
}

what is this? I did not see you define this.

and voltCap is too big. You need understand what the code mean and modify accordingly. Not just copy and paste.

int acc = 2; is too big too.

1 Like

I had voltCap capped at 12, however when i changed acc to .04 and made it a float nothing happened. Setting the voltCap to 300 and then multiplying it by .04 before feeding the value to the motors seemed to work decently, and acc worked at 2. I will try to change this back later to see if thats causing the problem.

LRotationSens and RRotationSens are just the right and left side rotation sensors, I should have included the definitions of those, sorry.

most of the variables should be double. Please check the VEX API document to match the data type.

Alright, I changed those variables to double and changed the voltCap back to 12. The error is now reading correctly, but the voltage is not changing. I think it has something to do with signRight and signLeft not being correct, do these also need to be doubles? I really just need them to be 1, 0 or -1 but I think they might be ending up as always 0

(changed code)

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() {
  
  double errorLeft;          //Error of left drive motors: target left value - actual left value
  double errorRight;         //Error of right drive motors: target right value - actual right value

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  double kp = .01;         //Proportional: multiplied by the error to determine the voltage rate
  double kpTurn = .02;      //Tune these if the robot is oscillating at the target value; if the robot does not stop after the movement is complete

  double acc = .04;        //Acceleration: how fast the voltage will increase over time; tune this value if robot is moving too quickly or slowly

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  double voltageLeft = 0;    //Variables used to tell the motors what voltage to run at
  double 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,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 {
      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 > 12) {           //Caps the voltage
      voltCap = 12;
    }

    if (fabs(voltageLeft) > voltCap) {                   //Makes sure the voltage of the left side is in the correct direction and
      voltageLeft = (voltCap) * signLeft;       //converts the voltage from 0<V<100 to -12<x<12 to give to the motors
    }

    if (fabs(voltageRight) > voltCap) {                  //Makes sure the voltage of the right side is in the correct direction and
      voltageRight = (voltCap) * signRight;     //converts the voltage from 0<V<100 to -12<x<12 to give to the motors
    }

    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
}

I did not see any obvious issue here. You can print the value of each variable and check they are what you expected.

Clear problem!!! (errorLeft > 0) and (errorRight > 0) are booleans, meaning they evaluate to true or false. I don’t know why you would want to say signLeft = true - false, or why you think it would make sense, but it doesn’t make sense to me to add and subtract true and false. Something with computer true being 1 might make it not come up with an error, but it’s probably not good to try it. The way you calculate the sign is signLeft = errorLeft / fabs(errorLeft). See if you can figure out why before you just copy it into your code, but I will tell you fabs() is the absolute value (take the minus sign off) function for doubles and floating point numbers and abs() is the same for integers.

1 Like

Actually, that is a functioning implementation of the sign function. More on it here

1 Like