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