PID has other affects

VEX V5 General Discussion General Forum
#1

So I made a PID to keep the claws on my robot at all times. The PID works, as when i move the claws they stay equal but all my driver control ability is gone. Can someone check over my code to see if the PID loop interferes with other things? Thanks.

task usercontrol()
{
	// User control code here, inside the loop
	float kp = 0.12;
	float ki = 0.00000002;
	float kd = 0.15;
	float current = 0;
	float totalerror;
	float lasterror;
	float errorright;
	float errorleft;
	float integral;
	float proportional;
	float derivative;
	float centerclawleft = 1715;
	float centerclawright = 3800;
	float error;
	while (true)
	{
		motor[chassisleft]  = (vexRT[Ch2]);
		motor[chassisright] =  (vexRT[Ch3]);


		if(vexRT[Ch2Xmtr2] > -15 &&  vexRT[Ch2Xmtr2] <15 )
		{

			motor[leftlift] = 5;  //lift nothing
			motor[rightlift] = 5;
			motor[liftleft] = 5;
			motor[liftright] = 5;
		}
		else
		{
			motor[leftlift] = (vexRT[Ch2Xmtr2]);   //lift up
			motor[rightlift] = (vexRT[Ch2Xmtr2]);
			motor[liftleft] = (vexRT[Ch2Xmtr2]);
			motor[liftright] = (vexRT[Ch2Xmtr2]);
			motor[llift]=(vexRT[Ch2Xmtr2]);
			motor[rlift]=(vexRT[Ch2Xmtr2]);
		}


		if(vexRT[Btn6UXmtr2] == 1)
		{
			motor[clawright]=127;
		}
		else if(vexRT[Btn6DXmtr2] == 1)  //claw
		{
			motor[clawright]=-127;
		}
		while(true) //pid loop
		{
			float errorright = SensorValue(rightpot) - centerclawright;//error for each claw
			float errorleft = centerclawleft - SensorValue(leftpot);
			totalerror += error;


			if(errorleft != errorright )
			{
				error= errorright - errorleft;
				
				if(totalerror > 25)
				{
					totalerror = 25;
				}
				proportional = error * kp;
				integral=totalerror * ki;
				derivative = (error-lasterror) * kd;
				lasterror=error;
				current = proportional + integral + derivative;
				motor[clawleft] = current ;
				wait1Msec(20);
			}
	if(errorleft == errorright)
	{
		totalerror=0;
			}
	}
#2

While you might have other problems, you have a while (true) loop within your other while (true) loop, so PID is continuously running, and you never break out of that inner while loop. You could do something along the lines of while (!vexRT[Btn6UXmtr2] == 1 && !vexRT[Btn6DXmtr2]) for your PID loop, so when you press a claw control button PID breaks out. This will, however, not align the sides when you’re pressing a button, so you’ll probably need to have a master and slave system instead of a sync system or somehow change your error when you’re pressing a button instead of setting motor speeds. We use a more bang bang approach to claw sync, which I can send you if you’re interested.

#3

ok, thanks. I change it from while(true) to if(1==1) and that solved the problem. Thanks for your help.

#4

No problem, but if (1==1) is always true, so you can just have nothing around it (remove the if statement essentially)

#5

My teams use similar programming to hold the position of a lift. The idea is to have a global variable that holds the desired position. When the driver moves the lift it continues to set that variable equal to the sensor value. When the lift is not being run by the driver then the PID kicks in and evaluates the position of the lift relative to the global variable and holds it there.

To paraphrase your code:

		if(vexRT[Btn6UXmtr2] == 1)
		{
			motor[clawright]=127;
                        centerclawright = SensorValue(rightpot);
                        centerclawleft = SensorValue(leftpot);
		}
		else if(vexRT[Btn6DXmtr2] == 1)  //claw
		{
			motor[clawright]=-127;
                        centerclawright = SensorValue(rightpot);
                        centerclawleft = SensorValue(leftpot);
		}
		else  //( instead of while) runs then pid loop when the buttons aren't pressed.
		{
			float errorright = SensorValue(rightpot) - centerclawright
                        ...

You could later add some claw preset locations on your 7 or 8 block of buttons which sets the centerclawright and centerclawleft to specific positions.

#6

Instead of wrapping the PID code in a while loop, make a separate task and start it inside the usercontrol task like below.

If you don’t know what a task is, it essentially runs side by side with another task (in this case, the usercontrol task). The reason why you have to run the PID loop alongside this in a task is because if you don’t, then you will constantly be running the PID in your while loop rather than running your other drive code as well.

Currently, your code in pseudocode is this:


task usercontrol() {
 while(true) {
    // driver control code
    while(true) {
        // PID code, and this loop runs forever while not allowing you to access your driver control code in the main part of the first while loop.
    }
  }   
}

This is what it should be:


	float kp = 0.12;
	float ki = 0.00000002;
	float kd = 0.15;
	float current = 0;
	float totalerror;
	float lasterror;
	float errorright;
	float errorleft;
	float integral;
	float proportional;
	float derivative;
	float centerclawleft = 1715;
	float centerclawright = 3800;
	float error;
task PIDTask() 
{
while(true) //pid loop
		{
			float errorright = SensorValue(rightpot) - centerclawright;//error for each claw
			float errorleft = centerclawleft - SensorValue(leftpot);
			totalerror += error;


			if(errorleft != errorright )
			{
				error= errorright - errorleft;
				
				if(totalerror > 25)
				{
					totalerror = 25;
				}
				proportional = error * kp;
				integral=totalerror * ki;
				derivative = (error-lasterror) * kd;
				lasterror=error;
				current = proportional + integral + derivative;
				motor[clawleft] = current ;
				wait1Msec(20);
			}
	if(errorleft == errorright)
	{
		totalerror=0;
	}
}
task usercontrol()
{
	// User control code here, inside the loop
        startTask(PIDTask) // start the PID task
	while (true)
	{
		motor[chassisleft]  = (vexRT[Ch2]);
		motor[chassisright] =  (vexRT[Ch3]);


		if(vexRT[Ch2Xmtr2] > -15 &&  vexRT[Ch2Xmtr2] <15 )
		{

			motor[leftlift] = 5;  //lift nothing
			motor[rightlift] = 5;
			motor[liftleft] = 5;
			motor[liftright] = 5;
		}
		else
		{
			motor[leftlift] = (vexRT[Ch2Xmtr2]);   //lift up
			motor[rightlift] = (vexRT[Ch2Xmtr2]);
			motor[liftleft] = (vexRT[Ch2Xmtr2]);
			motor[liftright] = (vexRT[Ch2Xmtr2]);
			motor[llift]=(vexRT[Ch2Xmtr2]);
			motor[rlift]=(vexRT[Ch2Xmtr2]);
		}


		if(vexRT[Btn6UXmtr2] == 1)
		{
			motor[clawright]=127;
		}
		else if(vexRT[Btn6DXmtr2] == 1)  //claw
		{
			motor[clawright]=-127;
		}
		sleep(20);
	}

Also, remember to always put delays at the end of your tasks/loops, because otherwise you’ll run into some problems with one task always running and never the other.

A good video about tasks is by Cody here:
https://www.youtube.com/watch?v=oGG5-yvDM7Y

#7

Great explanation @M8R ,

Yes please watch this, I feel you’ll greatly benefit from it. Hopefully you can get over my sharp brassness.

#8