///// INITIALIZE ROBOT /////
void initializeRobot() {
// Stop All Motors //
motor[leftFrontMotor] = 0;
motor[leftRearMotor] = 0;
motor[rightFrontMotor] = 0;
motor[rightRearMotor] = 0;
motor[spinnerMotor] = 0;
motor[liftMotor] = 0;
motor[leftWinch] = 0;
motor[rightWinch] = 0;
/* Commented out to test servo on new robot for 2014 year
// Medium hook rate and retracted position
SetHookServos(HOOK_MIN);
*/
servoChangeRate[leftHook] = 2;
servoChangeRate[rightHook] = 2;
/* Commented out for same reason as above
// Low hopper speed and retracted position
SetHopperServos(HOPPER_MAX);
*/
servoChangeRate[leftHopper] = 1;
servoChangeRate[rightHopper] = 1;
// Initialize encoders
resetDriveEncoder();
// Enable the light sensors
FlashLights(1, 0);
}
void
TCDSti::LedSmile(void)
{
if (m_ControllerType == C51134)
m_pPcio->Dome_ioWrite(DIAG_LIGHTDELAY_134, (SHORT) 1);
else
m_pPcio->Dome_ioWrite(DIAG_LIGHTDELAY_140, (SHORT) 1);
ClearLightingModeAll();
SetLightingModeCol(1);
SetLightingModeRow(1);
SnapShot(150);
SetLightingModeCol(9);
SetLightingModeRow(9);
FlashLights();
DomeReset();
}
