void PressBumperToContinue()
{
nxtScrollText("");
nxtScrollText("press button");
nxtScrollText("to continue");
PlaySoundFile("! Attention.rso");
while (!TSreadState(BUMPER))
wait1Msec(10); // not pressed
while (TSreadState(BUMPER))
wait1Msec(10); // pressed
wait1Msec(500);
}
void Calibrate()
{
T("enter Calibrate");
LLinit(LINELEADER);
nxtScrollText("Place over white");
PressBumperToContinue();
wait1Msec(500);
LLcalWhite(LINELEADER);
nxtScrollText("");
nxtScrollText("Place over line");
nxtScrollText("(Black)");
PressBumperToContinue();
wait1Msec(500);
LLcalBlack(LINELEADER);
// compass calibration
nxtScrollText("");
nxtScrollText("compass calibrate");
PressBumperToContinue();
wait1Msec(500);
motor[LEFT] = 30;
motor[RIGHT] = -30;
wait1Msec(500);
HTMCstartCal(COMPASS);
wait1Msec(5000);
HTMCstopCal(COMPASS);
motor[LEFT] = 0;
motor[RIGHT] = 0;
PlaySound(soundFastUpwardTones);
T("leave Calibrate");
wait1Msec(2000);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
while(true)
{
bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase.
eraseDisplay();
// WriteShort(hFileHandle, nIoResult, nParm);
if ( externalBatteryAvg < 0)
{
while (externalBatteryAvg < 0)
{
eraseDisplay();
nxtDisplayTextLine(1, "Ext Batt: OFF"); //External battery is off or not connected
nxtDisplayCenteredBigTextLine(1, "What!?");
nxtDisplayTextLine(3, "My name is,");
nxtDisplayTextLine(4, "Iego Montoya,");
nxtDisplayTextLine(5, "you did not turn");
nxtDisplayTextLine(6, "on the robot");
nxtDisplayTextLine(7, "NOW YOU DIE");
nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!");
PlayImmediateTone(600, 20);
PlayImmediateTone(400, 20);
wait10Msec(20);
}
}
else
{
nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);
}
while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7)
{
eraseDisplay();
nxtDisplayBigTextLine(6, "Battery");
nxtScrollText("poopheads");
nxtScrollText("You didnt change the battery you shmuts!");
nxtScrollText("the battery you");
nxtScrollText("shmuts!");
PlayImmediateTone(600, 70);
PlayImmediateTone(400, 70);
wait10Msec(20);
}
nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage
nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]);
nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1));
nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]);
nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]);
wait10Msec(20);
}//End of While
//Actuation tests.
motor[R_Motor] = 100;
wait10Msec(30);
motor[R_Motor] = 0;
wait10Msec(30);
motor[L_Motor] = 100;
wait10Msec(30);
motor[L_Motor] = 0;
wait10Msec(30);
motor[Flag_Raiser] = 100;
wait10Msec(30);
motor[Flag_Raiser] = 0;
wait10Msec(20);
motor[Winch] = 100;
wait10Msec(30);
motor[Winch] = 0;
wait10Msec(30);
motor[Arm] = 100;
wait10Msec(30);
motor[Arm] = 0;
wait10Msec(30);
motor[Block_Sweep] = 100;
wait10Msec(30);
motor[Block_Sweep] = 0;
wait10Msec(30);
servo[Bucket_Release] = 100;
wait10Msec(30);
servo[Bucket_Release] = 0;
wait10Msec(30);
servo[Spring_Release] = 100;
wait10Msec(30);
servo[Spring_Release] = 0;
wait10Msec(30);
servo[IRS_1] = 100;
wait10Msec(30);
servo[IRS_1] = 0;
wait10Msec(30);
}
void T(string t)
{
nxtScrollText(t);
writeDebugStreamLine(t);
}
