//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);
}
}
}