# Drive PID criticism

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;

{
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) {