Several thoughts not in any particular order.
Edit: Are you using the latest ROBOTC version (3.60) ? If not, then I strongly suggest you upgrade, most (all?) upgrades have been free for the last couple of years as far as I know.
You are overloading the debugStream, this line
writeDebugStreamLine("Encoder2:%3d Encoder9:%3d \n",nMotorEncoder[port2], nMotorEncoder[port9]);
Will send 30 chars to the debug stream so if called at a 5mS rate that will be 6000 chars every second. The serial connection from the joystick to the PC is at 115200 baud or approximately 11000 chars every second. I will go into detail in another post as to why this theoretical rate will never be achieved, but suffice to say that any more than perhaps 30% of that rate would be pushing ROBOTC a little. You have a couple of options here, you could drop the update rate to every 10mS or you could reduce the amount of data send for each call. The labels “Encoder2:” and “Encoder9:” are superfluous so I would drop them out. I tested this and it ran at a 5mS rate.
void LogEncoder()
{
static long l = 0;
writeDebugStreamLine("%5d 2:%3d 9:%3d",l++, nMotorEncoder[port2], nMotorEncoder[port9]);
}
19 chars every 5mS so 3800 chars every second, it’s near the limit for ROBOTC. I added the counter so that even if one or two samples were lost you could tell where each sample was supposed to be and create a more accurate graph.
There’s a small error in the mail loop.
PIDLeft.Output = UpdatePIDController(PIDLeft,300-nMotorEncoder[port9]);
PIDRight.Output = UpdatePIDController(PIDLeft,300-nMotorEncoder[port2]);
You send PIDLeft to both update calls, one should be PIDRight
Decide where you want to set the Output value, currently you set it both in the UpdatePIDController as well as assigning it using a return value.
You are using the “old style” ROBOTC method to send the PID data as a pointer to the function, it’s not wrong (perhaps you have an older ROBOTC version) but normal C code convention would be.
int UpdatePIDController(TPID *PIDController, int Error)
{
PIDController->Error = Error;
PIDController->Integral = (PIDController->Integral * (3 / 4)) + PIDController->Error;
PIDController->Derivative = PIDController->Error - PIDController->LastError;
PIDController->LastError = PIDController->Error;
PIDController->Output = ((float) PIDController->Error * PIDController->Kp)
+ (PIDController->Integral * PIDController->Ki)
+ (PIDController->Derivative * PIDController->Kd);
return PIDController->Output;
}
and the calls
PIDLeft.Output = UpdatePIDController( &PIDLeft, 300-nMotorEncoder[port9]);
PIDRight.Output = UpdatePIDController( &PIDRight, 300-nMotorEncoder[port2]);
Also, this line
PIDController->Integral = (PIDController->Integral * (3 / 4)) + PIDController->Error;
has a problem with the multiply of (3/4). As they are in parenthesis, they will be calculated first and, being integers, will evaluate to 0. You can check if this happens by looking at the assembly code listing, here is the relevant section.
PIDController->Integral = (PIDController->Integral * (3 / 4)) + PIDController->Error;
0007: AA29060045000E000000 S06(short) = *PIDController:*S00+14(short *) * 0
0011: 800645000C S06(short) += PIDController:*S00+12(short *)
0016: 1245000E290600 PIDController:*S00+14(short *) = S06(short)
Notice the multiply by 0 in line 7. So either use 0.75 or remove the parenthesis and do as two operations.
PIDController->Integral = (PIDController->Integral * 0.75) + PIDController->Error;
or
PIDController->Integral = (PIDController->Integral * 3 / 4) + PIDController->Error;
To detect the position being achieved, if you are running the PID as a sort of one time function, then detect the error being below some threshold, perhaps 10% of the intended movement. However, often the PID loop is run continuously and the target position adjusted by user code. The error is then constantly calculated and if it is large enough causes the motors to be driven. Anyway, here is some revised code with the bugs out, I did not change any of the overall functionality, I will leave that to you to decide how best to do it.
typedef struct
{
float Kp;
float Ki;
float Kd;
int Error;
int Integral;
int Derivative;
int LastError;
int Output;
} TPID;
TPID PIDLeft;
TPID PIDRight;
int UpdatePIDController(TPID *PIDController, int Error)
{
PIDController->Error = Error;
PIDController->Integral = (PIDController->Integral * 3 / 4) + PIDController->Error;
PIDController->Derivative = PIDController->Error - PIDController->LastError;
PIDController->LastError = PIDController->Error;
PIDController->Output = ((float) PIDController->Error * PIDController->Kp)
+ (PIDController->Integral * PIDController->Ki)
+ (PIDController->Derivative * PIDController->Kd);
return PIDController->Output;
}
void LogEncoder()
{
static long l = 0;
writeDebugStreamLine("%5d 2:%3d 9:%3d",l++, nMotorEncoder[port2], nMotorEncoder[port9]);
}
void leftside(int speed)
{
motor[port8]=speed;
motor[port9]=speed;
}
void rightside(int speed)
{
motor[port2]=speed;
motor[port3]=speed;
}
task main()
{
PIDLeft.Kp=0.5;
PIDLeft.Ki=0;
PIDLeft.Kd=0;
PIDRight.Kp=0.5;
PIDRight.Ki=0;
PIDRight.Kd=0;
// clear debug window if you want
wait1Msec(1000);
clearDebugStream();
while(true)
{
PIDLeft.Output = UpdatePIDController( &PIDLeft, 300-nMotorEncoder[port9]);
PIDRight.Output = UpdatePIDController( &PIDRight, 300-nMotorEncoder[port2]);
LogEncoder();
leftside(PIDLeft.Output);
rightside(PIDRight.Output);
wait1Msec(5);
}
}