Drive PID criticism

VEX V5 General Discussion Technical Discussion
1

Hi!
So I’ve written a task to run PID for my drive. (Let me know if you’d like the whole code, at the moment I am just going to c/p the stuff that’s actually necessary to understand what’s going on). I have not tested this, and I need some fresh eyes to take a look at it and tell me what may potentially go wrong, some tips on starter constants, and potentially whole different ways of turn error correction.
Thanks a lot, guys.

DriveR and DriveL are for the two respective sides of the drive.


///////////////////////////////////
//Robot-based Definitions//
//////////////////////////////////

#define DRIVE_TYPE "dropcenter"
#define TPR 360
#define WHEEL_SIZE 5
#define WHEEL_CIRC 15.70
#define LCD_LEFT 1
#define LCD_CENTER 2
#define LCD_RIGHT 4

////////////////////////////////////////////////////////////////////////
//DRIVE PID, BASIC CONVERSION AND INIT FUNCTIONS//
///////////////////////////////////////////////////////////////////////

void driveR(int power)
{
	motor(DriveLL) = motor(DriveLR) = (val > 127) ? 127 * sgn(power) : power;
}
void driveL(int power)
{
	motor(DriveRL) = motor(DriveRR) = (val > 127) ? 127 * sgn(power) : power;
}
void gyroInit()
{
	SensorType[in1] = sensorNone;
	wait1Msec(1000);
	SensorType(in1) = sensorGyro;
	wait1Msec(2000);
}

float inchTicks(float inch) {
	float ticks = TPR * (inch / WHEEL_CIRC);
	return ticks;
}

float ticksInch(float sensor) {
	return (sensor / TPR) * WHEEL_CIRC;
}

float target = 0;
int maxDrivePower = 127;

task fwdDrivePID()
{
	float Kp = 0.3;
	float Ki = 0.5;
	float Kd = 1.5;
	float proportional = 0;
	float derivative = 0;
	float integral = 0;
	float error = 1;
	float prevError = 0;
	float iLimit = 100;
	long lEncoder, rEncoder;
	int speed;
	float dT = time1(T1) - t;

	float turnError;
	float turnPower;
	long turnTarget = 0;
	float turnKp = 0.34;
	float turnRatio = 7;

	while (speed > 0) { // Is saying speed > 0 a good way of letting the loop escape once the robot's done so I can start another instance of this task?
		lEncoder = SensorValue(encLeft);
		rEncoder = SensorValue(encRight);
		
		error = target - ((lEncoder + rEncoder) / 2); // error = target - avg of both encoders
		turnError = turnTarget - (rEncoder - lEncoder);
		proportional = error;

		integral = (abs(error) <= 0.25 * target) ? integral + (error*dT) : 0; // only use integral if the error is less than 25% of the target (more simply if the robot is 75% of the way through driving)
		integral = (integral >= iLimit) ? 0 : integral; // limit the integral to 100 and reset it if it gets to 100.
		
		derivative = (error - prevError) / dT;
		
		turnPower = ((turnKp * turnRatio) * turnError);
		speed = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
		
		if (abs(speed) > 127) {
			speed = sgn(speed) * maxDrivePower;
		}

		DriveR(speed + turnPower); //How might I account for if both speeds are at 127 and can't be changed anymore to account for turning?
		DriveL(speed - turnPower);
		prevError = error;

		wait1Msec(10);
		t += dT;
///		if (error == 0) {
///			driveSet(0, 0);
///			lEncoder = 0;
///			rEncoder = 0; 
///				return; 
///		}
	}
}

void fwdDriveInch(float inch) {
	stopTask(fwdDrivePID);
	target = inchTicks(inch);
	startTask(fwdDrivePID);
}

P.S. If you’re willing to give me a hand but need me to tell you what’s going on because of my lack of comments, please let me know.

2