P loop debugging

Programming Support VEXcode Pro V5 Text Tech Support
,
#1

I’ve been wanting to use a PD loop for our team’s drivetrain during auton. Before adding the Derivative, the P loop was off. When we tested a negative value it would cause the bot to move forward about 2-4 feet, then move back 2-4 feet. How can it be changed to move in a single direction?

The code:

void resetDriveSensors (){
  leftBack.setPosition(0, degrees);
  rightBack.setPosition(0, degrees);
  leftFront.setPosition(0, degrees);
  rightFront.setPosition(0, degrees);
}

void lateralMovement(double targetValue){

    // Gets the position of the motor in degrees
    int leftMotorPosition = leftFront.position(degrees);
    int rightMotorPosition = rightFront.position(degrees);
    int leftBackMotorPosition = leftBack.position(degrees);
    int rightBackMotorPosition = rightBack.position(degrees);

    // Divides the positions to get the average
    int averagePosition = (leftMotorPosition + rightMotorPosition + leftBackMotorPosition + rightBackMotorPosition)/4;


    double error = targetValue - averagePosition;

    double Kp = 0.5;

    double lMotorSpeed;

  while (fabs(error)>4){
    

    lMotorSpeed = Kp*error;

    rightFront.spin(forward, lMotorSpeed, pct);
    leftFront.spin(forward, lMotorSpeed, pct);
    leftBack.spin(forward, lMotorSpeed, pct);
    rightBack.spin(forward, lMotorSpeed, pct);

    error = targetValue - averagePosition;
  }
  leftFront.stop(brake);
  leftBack.stop(brake);
  rightBack.stop(brake);
  rightFront.stop(brake);
}

We are using the motor encoders because all of the tri-ports are used and we don’t have an extender. This while, while (fabs(error)>4), is so that there is some wiggle room so it have to get the error all the way to zero.

#2

One issue I see is that you are not recalculating the average values inside of the loop. Also, if it’s oscillating your kp value is likely too high.

#3

Would this be better? Also I currently don’t have our bot to see if it oscillating

bool latDriveOn = false;

void LatDrive(double targetDistance){

  double Kp = 0.5;

  while(latDriveOn){
    int bakLefPos = leftBack.position(degrees);// gets the position of the left back motor to be used for an average 
    int bakRigtPos = rightBack.position(degrees);// gets the position of the right back motor to be used for an average
    int frntLefPos = leftFront.position(degrees);// gets the position of the left front motor to be used for an average
    int frntRigtPos = rightFront.position(degrees);// gets the position of the right front motor to be used for an average

  
    int avgPos = (bakLefPos + bakRigtPos + frntLefPos + frntRigtPos)/4;
  
    double error = targetDistance - avgPos;

    double latMotorSpeed = error * Kp;

    if (targetDistance < 0){
      leftFront.spin(reverse, latMotorSpeed, pct);
      leftBack.spin(reverse, latMotorSpeed, pct);
      rightFront.spin(reverse, latMotorSpeed, pct);
      rightFront.spin(reverse, latMotorSpeed, pct);
    } else if (targetDistance > 0) {
      leftFront.spin(forward, latMotorSpeed, pct);
      leftBack.spin(forward, latMotorSpeed, pct);
      rightFront.spin(forward, latMotorSpeed, pct);
      rightFront.spin(forward, latMotorSpeed, pct);  
    }
  }
  leftFront.stop(brake);
  leftBack.stop(brake);
  rightFront.stop(brake);
  rightFront.stop(brake);
}
#4

Yes that would work. You don’t need the if statements because if error is negative the motors will spin backwards anyway.

2 Likes