task main() { initializeRobot(); State currentState; memset(¤tState, 0, sizeof(State)); //waitForStart(); // Wait for the beginning of autonomous phase. while (currentState.IRBeaconDir != 8) { getLatestInput(¤tState); handleIRInputs(¤tState); updateAllMotors(¤tState); } while (currentState.touched != 1) { getLatestInput(¤tState); handleColorInputs(¤tState); handleTouchInputs(¤tState); updateAllMotors(¤tState); } }
void command (State *state, int dir, int driveSpeed, int armSpeed, int wristPos, int liftSpeed, int time) { state->desiredDriveDirection = dir; state->desiredDriveSpeed = driveSpeed; state->armSpeed = armSpeed; state->wristPosition = wristPos; state->liftSpeed = liftSpeed; computeDriveMotorSpeeds(state); updateAllMotors(state); wait1Msec(time); state->desiredDriveDirection = -1; state->desiredDriveSpeed = 0; state->armSpeed = 0; state->liftSpeed = 0; }
task main() { initializeRobot(); State actualState; State desiredState; // Initialize everything in the desired state to 0. memset(&desiredState, 0, sizeof(desiredState)); desiredState.wristPosition = 100; desiredState.desiredDriveSpeed = DRIVESPEED; //waitForStart(); // wait for start of tele-op phase while (true) { // Wait for the next update from the joystick. UserInput input; getLatestInput(&desiredState, &input); // Process the joystick input handleDriveInputs(&desiredState, &input); handleArmInputs(&desiredState, &input); handleWristInputs(&desiredState, &input); handleLiftInputs(&desiredState, &input); handleTineInputs(&desiredState, &input); handleOutriggerInputs(&desiredState, &input); computeDriveMotorSpeeds(&desiredState); computeActualState(&desiredState, &actualState); updateAllMotors(&actualState); // Display variable values for debugging purposes showDiagnostics(&desiredState, &actualState, &input); } }