@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.24 ticks
//1 turn of wheel 4pi inches
//4 wheels with IMEs
//4pi inches = 4627.22ticks
//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);
}
}
}