Drive Straight PID Loop

VEX V5 General Discussion General Forum
2

I think this worked. This was for encoder backs, so you will need to change nMoterEncoder] to SensorValue() for OSE’s.


// function to run drive motors for specified distance in units defined by WHEEL_DIAM then stop the drive motors
// +dist = forward, -dist = reverse.
void driveDistance(int dist)
{
	nMotorEncoder[drive_LF] = 0; // reset the ime values to 0
	nMotorEncoder[drive_RF] = 0;
	driveLastError = 0; // update driveLastError

	driveTarget = fabs(dist/(WHEEL_DIAM*PI) * ENCODER_TICKS); // convert distance into encoder ticks. WHEEL_DIAM and ENCODER_TICKS set in globals
	driveError = driveTarget - fabs(nMotorEncoder[drive_LF]); // initial error calculation

	while (fabs(driveError) > DRIVE_ERROR_THRESH) // while more than DRIVE_ERROR_THRESH from target
	{
		driveError = driveTarget - fabs(nMotorEncoder[drive_LF]); // update error value

		int drivePower = sgn(dist) * (drivePD_Kp * driveError) + (drivePD_Kd * (driveError - driveLastError)); // initial PD power calculation

		drivePower = fabs(drivePower) > DRIVE_MAX_PWR ? sgn(drivePower) * DRIVE_MAX_PWR : drivePower; // limit power if calc is over max

		// drive straight power modifier
		int rDiff = fabs(nMotorEncoder[drive_LF]) - fabs(nMotorEncoder[drive_RF]); // check for right encoder difference
		int rMod = sgn(rDiff)*drivePower*.1; 					// rMod = 10% drivePower in the direction of rDiff

		set_drivePower( drivePower , drivePower + rMod );

		driveLastError = driveError; // update driveLastError

		wait1Msec(25);
	}

	set_drivePower( 0 , 0 ); // stop motors after target is reached

}