task main()
{
if (SensorValue[leftBtn]==1 || SensorValue[rightBtn]==1)// satallite hit either end
{
moveInches(1);
}
}
task main()
{
writeDebugStreamLine("initalizing");
initializeRobot();
writeDebugStreamLine("initalized");
#ifdef COMPETITION
waitForStart();
#else
while (nNxtButtonPressed != kEnterButton){}
#endif
rotateDegrees(45);
while (nNxtButtonPressed != kRightButton){}
rotateDegrees(45);
while (nNxtButtonPressed != kRightButton){}
if (TestHook == 1) {
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
while (nNxtButtonPressed != kRightButton){}
hookGrab();
wait1Msec(3000);
moveInches(-20);
hookUngrab();
while (nNxtButtonPressed != kLeftButton){}
}
if (TestHardware == 1) {
writeDebugStreamLine("Test Hardware");
trapDoorOpen();
trapDoorClose();
servo[collector] = COLLECTOR_IDLE;
setPosition (tas, POS_BALL_COLLECTING, DEFAULT_DISTANCE);
wait10Msec(300);
servo[collector] = COLLECTOR_IN;
wait10Msec(300);
servo[collector] = COLLECTOR_IDLE;
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
wait10Msec(1000);
trapDoorOpen();
wait10Msec(3000);
}
if (TestPositions == 1) {
writeDebugStreamLine("Test Positions");
// while (nNxtButtonPressed != kRightButton){}
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kLeftButton){}
// setPosition (tas, POS_AT_60CM, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kRightButton){}
//Test 90
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kLeftButton){}
// setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
// while (nNxtButtonPressed != kRightButton){}
// trapDoorOpen();
// while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while(nNxtButtonPressed != kEnterButton){}
moveInches(24);
while (nNxtButtonPressed != kRightButton){}
trapDoorOpen();
while(nNxtButtonPressed != kEnterButton){}
moveInches(-24);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE+3.5);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
trapDoorOpen();
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_30CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kLeftButton){}
//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
//while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kRightButton){}
setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
//while (nNxtButtonPressed != kLeftButton){}
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
while (nNxtButtonPressed != kEnterButton){}
}
#if 0
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
wait10Msec(200); //wait 2 sec for raise
//moveInches(50);
moveInches(54);
setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
wait10Msec(1000);
//moveInches(14);
moveInches(10);
trapDoorOpen();
wait10Msec(500);
moveInches(-20);
setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
wait10Msec(2000);
#endif
}
task main()
{
moveInches(5,3);
}
