PID support

@jpearman We’re trying to use PID system for our robot but the error is not correcting the flywheel speed and whenever we press a button the flywheel goes on an on non-stop full speed. Is there anything we can do to make it work better?

#pragma config(Sensor, dgtl1, wheelEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl12, greenLED, sensorLEDtoVCC)
#pragma config(Motor, port1, liftRight, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, frontLeft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port3, frontRight, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port4, backLeft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port5, backRight, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port6, shootLeft, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port7, shootRight, tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor, port8, shootLeftTwo, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port9, shootRightTwo, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port10, liftLeft, tmotorVex393_HBridge, openLoop)
int target=0; //define target and rpm at global level so it can be passed between tasks
float rpm;
int currentEncoder; //Define variables to use
int previousEncoder;
int deltaEncoder;
int currentTime;
int previousTime;
int deltaTime;
task speedControl(); //define tasks to be used
task speedCalculate();
task main()
{
SensorValue[greenLED]=0;
startTask(speedControl);
startTask(speedCalculate);
while (true)
{
motor[frontRight] = vexRT[Ch3] - vexRT[Ch1] - vexRT[Ch4];
motor[backRight] = vexRT[Ch3] - vexRT[Ch1] + vexRT[Ch4];
motor[frontLeft] = vexRT[Ch3] + vexRT[Ch1] + vexRT[Ch4];
motor[backLeft] = vexRT[Ch3] + vexRT[Ch1] - vexRT[Ch4];

				if(vexRT[Btn6UXmtr2] == 1)
	{
		motor[liftLeft] = -127;
		motor[liftRight] = -127;

		target = 400;
	}
	else
	{


		if(vexRT[Btn5UXmtr2] == 1)
		{
			motor[liftLeft] = -127;
			motor[liftRight] = -127;
		}
		else
		{
			if(vexRT[Btn5DXmtr2] == 1)
			{
				motor[liftLeft] = 127;
				motor[liftRight] = 127;
			}
			else
			{
				motor[liftLeft] = 0;
				motor[liftRight] = 0;
			}
		}
	}
	
		if(vexRT[Btn6DXmtr2] == 1)
		{
			target = 200;
		}

		if(vexRT[Btn8UXmtr2] == 1)
		{
			target = 150;
		}

		if(vexRT[Btn8RXmtr2] == 1)
		{
			target = 100;
		}

		if(vexRT[Btn8DXmtr2] == 1)
		{
			target = 75;
		}

		if(vexRT[Btn8LXmtr2] == 1)
		{
			target = 50;
		}
		if(vexRT[Btn7RXmtr2] == 1)
		{
			target = 0;
		}
		if(abs(target-rpm)<15)			//Use LED to indicate when velocity is at target
		{
			SensorValue[greenLED]=1;
		}
		else
		{
			SensorValue[greenLED]=0;
		}
	}

}
task speedCalculate()
{

	while(true)
	{
		previousEncoder = SensorValue[wheelEncoder];					//Measre Encoder and time at beginning
		previousTime = nSysTime;
		wait1Msec(25);																				//Wait to measre changes
		currentEncoder = SensorValue[wheelEncoder];						//Measure Encoder and time after delay
		currentTime = nSysTime;
		deltaEncoder = currentEncoder - previousEncoder;			//Calculate changes
		deltaTime = currentTime - previousTime;
		rpm = (deltaEncoder*60)*(1000/deltaTime)/360;					//calculate rpm but converrt degress to rotations, ms to s, s to minutes
		wait1Msec(10);																				//Wait to allow
	}
}
task speedControl()
{
	float kp = 0.9;																					//Define Variables to use
	float kd = 0.07;
	float ki = 0.03;
	int error = 0;
	int previousError = 0;
	int integral = 0;
	int derivative = 0;
	int motorSpeed = 0;
	while(true)
	{
		error = target - rpm;																	//Determine how far away from target rpmis
		integral += error;																		//Accumulate Error to determine steady-state answer
		derivative = error - previousError;										//Determine rate error is changing
		previousError = error;
		motorSpeed = kp*error + ki*integral + kd*derivative;	//Use PID loop to determine motorspeed
		if(motorSpeed<0)																			//Keep motors from going backward
		{
			motorSpeed = 0;
		}
		if(motorSpeed>127)																		//Keep Motorspeed from going over 127
		{
			motorSpeed = 127;
		}
		motor[shootLeft] = motorSpeed;												//Set Motors
		motor[shootRight] = motorSpeed;
		motor[shootLeftTwo] = motorSpeed;												//Set Motors
		motor[shootRightTwo] = motorSpeed;
		wait1Msec(40);																				//Delay for motor speed to adjust to RPM
	}
}

I tested the code and it fundamentally does work although may need tuning. One important think to check is if the encoder is counting in the correct direction, the assumption is that positive motor power will make the encoder increase its count. Check for that in the ROBOTC debugger, if it is wrong then swap the cables around (ie. digital 1 cable goes to digital 2 and vice-verse). The next thing to check is if the measured rpm is correct, your target speed needs to be appropriate for the measured rpm, that will depend on exactly where the quad encoder is installed, for example, on the motor output or after some of the gears.