pidSnip.c

//--Tunable Variables--\\
float kP = 1.0;
float kI = 1.0;
float kD = 1.0;
float kL = 50.0;

int targetValue = 0;

//--Basic PID Loop--\\
task basicPID(){
	int error = 0;
	int pError = 0;
	int p = 0;
	int i = 0;
	int d = 0;

	while(true){
		error = targetValue - nMotorEncoder[Motor1];

		p = error;
		i = abs(i + error) < kL ? i+ error : sgn(i + error)*kL;
		d = error - pError;

		pError = error;

		wait1Msec(25);
	}
  motor[Motor1] = p*kP + i*kI + d*kD;
}