@Barin

I initiated the PID with button 5D for testing purposes

#pragma config(I2C_Usage, I2C1, i2cSensors)

#pragma config(Sensor, I2C_1, rightIEM, sensorQuadEncoderOnI2CPort, , AutoAssign )

#pragma config(Sensor, I2C_2, leftIEM, sensorQuadEncoderOnI2CPort, , AutoAssign )

#pragma config(Sensor, I2C_3, , sensorQuadEncoderOnI2CPort, , AutoAssign )

#pragma config(Sensor, I2C_4, , sensorQuadEncoderOnI2CPort, , AutoAssign )

#pragma config(Motor, port1, RightMotor1, tmotorVex393HighSpeed_HBridge, openLoop, encoderPort, I2C_1)

#pragma config(Motor, port2, RightMotor2, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)

#pragma config(Motor, port3, LeftMotor1, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_3)

#pragma config(Motor, port4, LeftMotor2, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_4)

void forward (int speed)

{

motor[RightMotor1] = speed;

motor[RightMotor2] = speed;

motor[LeftMotor1] = speed;

motor[LeftMotor2] = speed;

}

//627.2 ticks/revolution

//1 turn of the wheels - 4 inch wheels = 627.2*4 ticks*

//1 turn of wheel 4pi inches

//4 wheels with IMEs

//4*pi inches = 4*627.2*2ticks*

//ticks/inch = 627.24/(4*pi) = 199.64396

int Inches (float inch)

{

int ticks;

ticks = inch*199.64396;

return ticks;

}

int MillisecondsToSeconds (float RawSeconds)

{

int milliseconds;

milliseconds = RawSeconds*1000;

return milliseconds;

}

void PIDMovement (float target, float waitTime)

{

float Kp = 0.2;

float Ki = 0.05;

float Kd = 0.5;

```
int error;
float proportion;
int integralRaw;
float integral;
int lastError;
int derivative;
float integralActiveZone = Inches(3);
float integralPowerLimit = 50/Ki;
int finalPower;
bool timerBool = true;
nMotorEncoder[RightMotor1] = 0;
nMotorEncoder[RightMotor2] = 0;
nMotorEncoder[LeftMotor1] = 0;
nMotorEncoder[LeftMotor2] = 0;
clearTimer(T1);
while (time1[T1] < MillisecondsToSeconds(waitTime))
{
error = Inches(target)-(nMotorEncoder[RightMotor1]+nMotorEncoder[RightMotor2] + nMotorEncoder[LeftMotor1]+ nMotorEncoder[LeftMotor1]);
proportion = Kp*error;
if (abs(error) < integralActiveZone && error != 0) // float integralActiveZone = Inches(3); defined above
{
integralRaw = integralRaw+error;
}
else
{
integralRaw = 0;
}
if (integralRaw > integralPowerLimit) //float integralPowerLimit = 50/Ki; defined above
{
integralRaw = integralPowerLimit;
}
if (integralRaw < -integralPowerLimit)
{
integralRaw = -integralPowerLimit;
}
integral = Ki*integralRaw;
derivative = Kd*(error-lastError);
lastError = error;
if (error == 0)
{
derivative = 0;
}
finalPower = proportion+integral+derivative; //proportion+derivative+integral
forward(finalPower);
wait1Msec(40);
if (error < 30)
{
timerBool = false;
}
if (timerBool)
{
clearTimer(T1);
}
}
forward(0);
```

}

task main ()

{

```
while(true)
{
motor[RightMotor1] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
motor[RightMotor2] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
motor[LeftMotor1] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
motor[LeftMotor2] = (vexRT[Ch2] + vexRT[Ch1])/2; //(y+x)/2
if(vexRT[Btn5D] == 1)
{
PIDMovement(2,1);
}
}
```

}