Robot Keeps Shaking During Autonomous

Programming Support
#1

Hey everyone! I’ve recently been working on tuning my lateral PID and the robot moves to its inputted distanced, but then just spins forever. Do you know what the issue is and how to fix it? I also wanted to note that there are no current sensors on the robot. Thank you!

float dgs = 180/(3.25 * M_PI);

//Tuning Constants for Distance
double kP = 1;
double kI = 0;
double kD = 0;

//Tuning Constants for Turn
double turnkP = 2.0;
double turnkI = 0;
double turnkD = 0;

//Establish Variables Used in Control Loop
double desiredValue = 0;
int desiredTurnValue = 0;

int error;
int prevError = 0;
int derivative;
int totalError = 0;

int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;

// Allows You to Reset Your Loop
bool resetDriveSensors = false;

// Allows You to Turn Off PID Control in Driver Control
bool enableDrivePID = true;

// Drive pid
int drivePID() {

  while (enableDrivePID) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
      LFrontBottom.setPosition(0, degrees);
      LFrontTop.setPosition(0, degrees);
      LMiddle.setPosition(0, degrees);
      RFrontBottom.setPosition(0, degrees);
      RFrontTop.setPosition(0, degrees);
      RMiddle.setPosition(0, degrees);
      //Inertial.setHeading(0,degrees);
    }

    int LFrontBottomPos = LFrontBottom.position(degrees);
    int LFrontTopPos = LFrontTop.position(degrees);
    int LMiddlePos = LMiddle.position(degrees);
    int RFrontBottomPos = RFrontBottom.position(degrees);
    int RFrontTopPos = RFrontTop.position(degrees);
    int RMiddlePos = RMiddle.position(degrees);

    ////////////////////////////////////////////////////
    // Drive PID
    ////////////////////////////////////////////////////

    int averagePosition = (LFrontBottomPos + LFrontTopPos + LMiddlePos + RFrontBottomPos + RFrontTopPos + RMiddlePos)/6;

    //PROPRTION
    error = desiredValue - averagePosition;

    //DERIVATIVE
    derivative = error - prevError;

    //INTEGRAL
    totalError = totalError + error;

    double motorPower = (error * kP + derivative * kD + totalError * kI);

    ///////////////////////////////////////////////////
    // Turn PID
    ///////////////////////////////////////////////////

    int turnDifference = (LFrontBottomPos + LMiddlePos + LFrontTopPos) - (RFrontBottomPos + RMiddlePos + RFrontTopPos);

    //TURN PROPORTION
    turnError = desiredTurnValue - turnDifference;

    // turnError = DesiredTurnValue - Inertial.heading(degrees);
    
    //TURN DERIVATIVE
    turnDerivative = turnError - turnPrevError;

    //TURN INTEGRAL
    turnTotalError = turnTotalError + turnError;

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI);

    ///////////////////////////////////////////////////
    // Motor Commands
    ///////////////////////////////////////////////////

    double powerScaler = 1.0;

    double leftMotorPower = powerScaler * (motorPower - turnMotorPower);
    double rightMotorPower = powerScaler * (motorPower + turnMotorPower);

    LFrontBottom.spin(fwd, leftMotorPower, volt);
    LMiddle.spin(fwd, leftMotorPower, volt);
    LFrontTop.spin(fwd, leftMotorPower, volt);
    RFrontTop.spin(fwd, rightMotorPower, volt);
    RMiddle.spin(fwd, rightMotorPower, volt);
    RFrontBottom.spin(fwd, rightMotorPower, volt);

    prevError = error;
    turnPrevError = turnError;
    task::sleep(20);
  }

  return 1;
}

void LateralPID(int NumInches){
  resetDriveSensors = true;
  desiredValue = (dgs * NumInches);
}

void auton_left() {
  task PID(drivePID);
    LateralPID(-10);
    vex::task::sleep(100);
    desiredTurnValue = 90;
}
#2

I understand that you have commented this line out, because you do not have an inertial sensor yet, but you need it to make your code work. It is essential because it helps tell when the robot should stop turning - without it, the robot will never stop, just as you observed. The easiest and most accurate solution in the long run is simply to wait for the inertial sensor to arrive and uncomment the code then.

#3

They are currently using wheel encoders to calculate the turn difference which should still work.

Your turn difference calculation seems to have some issues. Try to find out how to calculate the angle of the robot from the difference in the wheels.
Also setting desired turn value to 0 to see if it drives straight.

#4

Yes, thank you. Dan is right - I misread the code.