Code and flywheel issues

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

I have been working on a flywheel similar to 8059, and it has been shooting just short of the goal. I have also been working on a Velocity PID algorithm, however it doesn’t stop the wheel, and always assumes maximum speed. My RPM calculation has also been climbing wildly upwards. I can’t tell if anything is working or how it goes wrong because the debugger on robotC won’t update any of the PID variables except for “loops” in a timely manner. The flywheel also makes all speeds above 90 the same, so we cant increase the speed to get the extra distance.

int fRPM = 0;//set in drivercontrol for flywheel speed.
int Ferror = 0;
	int Fpreverror = 0;
	int FdeltaT = 15;
	int Fprevtime = 0;
	float integral[4] = {0,0,0,0};
	float rollIntegral = 0;
	float Fderiv = 0;
	float trueRPM = 0;
	//clearTimer(T3);
	int loops = 0;
	float curve = 0;
task fRPMcontrol()
{
	motor[launcher4] = 0;
	motor[launcher3] = 0;
	motor[launcher2] = 0;
	motor[launcher1] = 0;
	float pgain = .0003;
	float igain = .0000;
	float dgain = .000;
	/*int Ferror = 0;
	int Fpreverror = 0;
	int FdeltaT = 15;
	int Fprevtime = 0;
	float integral[4] = {0,0,0,0};
	float rollIntegral = 0;
	float Fderiv = 0;
	float trueRPM = 0;
	*/clearTimer(T3);
	//int loops = 0;
	//float curve = 0;
	wait1Msec(15);
	while(true)
	{
		/*if(fRPM ==0)
		{
		SensorValue[flywheel] = 0;
		clearTimer(T3);
		wait1Msec(30);
		fSpeedControl(0);
		Fprevtime = 0;
		}
		else
		{*/
		if(loops > 3){loops = 0;}
		FdeltaT = time1(T3) - Fprevtime;
		Fprevtime = time1(T3);
		trueRPM = 11.6667 * (((SensorValue[flywheel] / 360) / (FdeltaT))/**1000*/ * 60);
		Ferror = fRPM - trueRPM;
		integral[loops] = Ferror;
		loops++;
		rollIntegral = (integral[0]+integral[1]+integral[2]+integral[3])/4;
		Fderiv = Ferror - Fpreverror;
		Fpreverror = Ferror;
		curve =  6 *((Ferror *pgain) + (rollIntegral * igain) + (Fderiv * dgain));
		if(curve >6)
		{
			curve = 6;
		}
		if(curve < -6)
		{
			curve = -6;
		}
		motor[launcher1] += motor[launcher1] + curve;
		motor[launcher2] += motor[launcher2] + curve;
		motor[launcher3] += motor[launcher3] + curve;
		motor[launcher4] += motor[launcher4] + curve;
		if(motor[launcher1] >120)
		{
			motor[launcher1] = 120; 
			motor[launcher2] = 120;
			motor[launcher3] = 120; 
			motor[launcher4] = 120; 
			
		}
		/*if(fRPM == 0)
		{
			motor[launcher1] = 0; 
			motor[launcher2] = 0;
			motor[launcher3] = 0; 
			motor[launcher4] = 0; 
			
		}*/
		//wait1Msec(30);
		if( abs(fRPM - trueRPM) <=50 )
		{
			SensorValue[flyIndicator] = 0;
		}
		else
		{
			SensorValue[flyIndicator] = 1;
		}
		wait1Msec(30);
	//}
	}

}
#2

here is my suggestion, your PID loop seems very complicated, more so than it needs to be.
to calculate flywheel velocity here is what I suggest, this code assumes that your encoder is on the same axle as the flywheel

Encoder = 0;
wait1Msec(20);
Dencoder = Encoder;

speed = Dencode/360 * 50 (WheelDiameterPI);
this way you don’t have to worry about using timers at all, and it greatly simplifies the code here.

I do see a couple of potential problems, you are not accumulating integral for more than 4 loop cycles, so that’s very little accumulation, your Dgain is zero so you have no derivative term, and finally, you have motors set this way

motor+= motor + curve;

this is not something I have ever seen
writing
motor += curve;

does the same thing as

motor = motor+curve;

so what you have is

motor = motor + motor + curve, and I don’t think that is what you want,

also not sure why you limit curve to 6 or -6 that seems like you are limiting a lot of potential change, and finaly I am not sure why your flywheel makes all values about 90 the same.

#3

I understand all of the syntax issues, but even if the speed went over, it would never decrease without killing the robot, and the RPM calculation was acting as if it were adding the speed every second.
The integral is in a list to get an average, which lowers more uniformly over time. I average them now, however, I could just increase the list size, and the errors would replace each other in the list as it rolls over.

#4

the speed cannot come down because of the line of code where you say

motors+= motors + curve

once motors reaches above 6, then it will never come down

think about a case where motors = 100;

you are saying

motors = motors +(motors + curve)

so that would be

motor = 100 + (100+ 6);

or maybe

motors = 100+ (100-6);

either way power still increases

you need to either

a) uncap curve

or
b) right the code as

motor+= curve;

#5

Okay, thanks. I’ll try that when I get a hold of my robot on Monday.

#6

the other reason your going to have infinite increases is because your measured velocity keeps increasing , this is because you never set the encoder on your flywheel to zero every time, you need to zero that encoder every time through the loop or you will get totally inccorect velocities

#7