Exemple #1
0
//Gyro turn to target angle
void gyroTurn(float target)
{
	if(abs(target) < 40)
		pidInit(gyroPid, 3, 0, 0.15, 3, 1270);
	bool atGyro = false;
	long atTargetTime = nPgmTime;
	long timer = nPgmTime;

	pidReset(gyroPid);
	gyroResetAngle();
	while(!atGyro)
	{
		//Calculate the delta time from the last iteration of the loop
		float dT = (float)(nPgmTime - timer)/1000;

		//Calculate the current angle of the gyro
		float angle = gyroAddAngle(dT);

		//Reset loop timer
		timer = nPgmTime;

		//Calculate the output of the PID controller and output to drive motors
		float error = (float)target - angle;
		float driveOut = pidExecute(gyroPid, error);
		driveL(-driveOut);
		driveR(driveOut);

		//Stop the turn function when the angle has been within 3 degrees of the desired angle for 350ms
		if(abs(error) > 3)
			atTargetTime = nPgmTime;
		if(nPgmTime - atTargetTime > 350)
		{
			atGyro = true;
			driveL(0);
			driveR(0);
		}
	}

	//Reinitialize the PID constants to their original values in case they were changed
	pidInit(gyroPid, 2, 0, 0.15, 2, 1270);
}
//gyro turn to target angle
void
gyroTurn(float fTarget){
	bool bAtGyro = false;
	long liAtTargetTime = nPgmTime;
	long liTimer = nPgmTime;
	float fGyroAngle = 0;

	while(!bAtGyro){
		//Calculate the delta time from the last iteration of the loop
		float fDeltaTime = (float)(nPgmTime - liTimer)/1000.0;
		//Reset loop timer
		liTimer = nPgmTime;

		float fGyroRate = gyroGetRate(gyro);
		fGyroAngle +=  fGyroRate * fDeltaTime;

		//					  -------------------------------<-
		//					 |				|
		//				 fgyroAngle 	fgyroRate 			 	Physical System
		//					 v				v
		// fTarget -> gyroAnglePID -> gyroRatePID -> motors--->
		//
		float driveOut = pidCalculate(gyroRatePid, pidCalculate(gyroAnglePid, fTarget, fGyroAngle), fGyroRate);

		driveL(-driveOut);
		driveR(driveOut);

		//Stop the turn function when the angle has been within 3 degrees of the desired angle for 350ms
		if(abs(fTarget - fGyroAngle) > 3)
			liAtTargetTime = nPgmTime;
		if(nPgmTime - liAtTargetTime > 350){
			bAtGyro = true;
			driveL(0);
			driveR(0);
		}
	}
}