Placing any sort of loop in your overridden functions (such as AutonomousPeriodic()) is discouraged.
Code off the top of my head:
Code:
// ...
float targetAngle;
int currentState;
float fowardStartTime;
const static int TURNING = 0;
const static int FORWARD = 1;
// ...
void AutonomousInit() {
targetAngle = gyro.GetAngle() + 45.0;
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.5);
currentState = FORWARD;
}
void AutonomousPeriodic() {
GetWatchdog().Feed();
if (currentState == TURNING) {
if (gyro.GetAngle() < targetAngle) {
Turn(); // Implement this function
} else {
currentState = FORWARD;
forwardStartTime = GetTime(); // Implement this function with your preferred method of time.
}
} else if (currentState == FORWARD) {
if (GetTime() < forwardStartTime + 3000)
DriveFoward(); // Implement this function.
else
currentState = -1;
}
}