Hello, I am writing a PID loop on RobotC, but have encountered a problem. My PID loop will start running and will go forward, but it wont stop.
I tried adding
if (error ==0)
{
stopAllMotors;
}
but not even that would make the motors stop running.
It’s unlikely that your error will ever be exactly 0. Try programming in a margin of error:
if(abs(error) < someNumber){
stopAllMotors();
}
where
someNumber
is a reasonably small number representing an acceptable margin of error.
@Barin I tried
if(abs(error) < Inches(.2))
{
stopAllMotors();
}
(I defined “Inches” earlier in the program to be the the readings of the tick value converted to be inches, so “Inches(.2)” would be an error with a magnitude of .2 inches, hence the name)
After adding that, the motors would still run continuously.
Could you post all your PID code? It’s hard to tell exactly what the problem might be.
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);
}
}
}
Just something simple you might want to try is reversing the value of the sensor you are using. I know that many times I have just had the value reversed, which make the motors be ever gaining error not loosing it.
@DanielIW26 That was the problem. Thank you!