void executeStep(Step *step){ writeDebugStreamLine("Beginning execution of step \"%s\"", step->name); switch (step->type) { case StepDrive: moveTo(step->target, step->power, step->forward); break; case StepTurnDrive: turnAndMoveTo(step->target, step->power, step->forward); break; case StepTurn: turnTo(step->target, step->power, step->forward); break; case StepGrab: grabGoal(); wait1Msec(500); break; case StepRelease: releaseGoal(); wait1Msec(500); break; case StepDone: writeDebugStreamLine("Finished executing Autonomous program"); StopAllTasks(); break; default: writeDebugStreamLine("Unknown step type detected (%d). Quitting now.", step->type); StopAllTasks(); } }
void executeGrab() { //bring the medium goal back to the parking zone drive(11800); grabGoal(); turnGyro(1200); if(getGyroNumber() > 650){ //Only continue the program if the gyro value is greater than 630 drive(-10700); turn(5500); releaseGoal(); } }