# P loop debugging

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.

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.

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

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

2 Likes