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