task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
//_______________________________________________________________________________________________________________________
time100[1] = 0;
liftBasket();
wait10Msec(DELAY*100); //wait for other side
float distance;
//nxtDisplayTextLine(1, "here 21" );
//while (nNxtButtonPressed != kLeftButton){}
if (LEFT == side)
distance = findBeacon(IR_LEFT);
else //if (RIGHT == side)
distance = findBeacon(IR_RIGHT);
//nxtDisplayTextLine(1, "here 22" );
//while (nNxtButtonPressed != kEnterButton){}
dropNramp(side, distance, color);
Close(hFileHandle, nIoResult);
}
task main ()
{
init();
int coor[2];
int x, y;
int dist;
int xA = 34;
int yA = 205;
int xB = 229;
int yB = 147;
int xC = 277;
int yC = 55;
int xHome;
int yHome;
//int xTerrain2 = 220;
bool foundBeacon;
/************* STEP 1 *************/
//Face the right direction
nxtDisplayTextLine(1, "Step 1");
//turnRight (0); // face north
turnRight (-45); // face north east
//turnRight(-90); // face east
//turnRight(-135); // face south east
//turnRight(180); // face south
//turnRight(135) // face south west
//turnRight(90); // face west
//turnRight(45) // face north west
wait1Msec(300);
/////////////////STEP 2 ///////////////
//Drive to y coordinate of A
nxtDisplayTextLine(2, "Step 2");
//Get home coordinates
getPos(coor);
x = xHome = coor[0];
//wait1Msec(1000);
y = yHome = coor[1];
dist = yA - y;
driveStraight(dist - 5); // Move to (A's y coordinate - 5)
wait1Msec(300);
////////////////STEP 3/////////////////
//Turn West
nxtDisplayTextLine(3, "Step 3");
turnRight(-90);
wait1Msec(300);
///////////////STEP 4/////////////////
//Move to x coord of A
nxtDisplayTextLine(4, "Step 4");
//Get current x coordinate
getPos(coor);
x = coor[0];
//Move to the same x coordinate as A - 10cm
dist = xA - x;
driveStraight(-(dist + 10));
wait1Msec(300);
//////////////STEP 5//////////////////
//Drop
// Look for magnetic beacon
nxtDisplayTextLine (5, "Step 5");
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5);
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to A and drop bins off
else {
getPos (coor);
dist = xA - coor[0];
driveStraight (-(dist + 5));
wait1Msec (300);
dropBins();
}
wait1Msec(300);
///////////////STEP 6///////////////////
//Move to x coordinate of B
getPos(coor);
dist = xB - coor[0];
driveStraight(-(dist+6));
wait1Msec(300);
//////////////STEP 7///////////////////
//Turn North
turnRight(90);
wait1Msec(300);
/////////////STEP 8////////////////////
//Move to Y coordinate of B
getPos(coor);
y = coor[1];
dist = yB - y; // Move to (B's y coordinate - 5)
driveStraight((dist - 5));
wait1Msec(300);
/////////////STEP 9///////////////////
// Drop bin at B
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5); //cm
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to B and drop bins off
else {
getPos (coor);
y = coor[1];
dist = yB - y;
driveStraight ((dist - 5));
wait1Msec (300);
dropBins();
}
wait1Msec(300);
///////////////STEP 10//////////////////////////
//Go to yC //Pass through rough terrain
getPos(coor);
y = coor[1];
dist = yC - y;
driveStraight(dist);
wait1Msec(300);
//////////////STEP 11/////////////////////////
//Turn East
turnRight (90);
wait1Msec(300);
///////////////STEP 12////////////////////////
//Drive to xC - 5cm
getPos (coor);
x = coor[0];
dist = xC - x;
driveStraight(dist - 5);
wait1Msec(300);
////////////////STEP 13/////////////////////
// Look for magnetic beacon
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5); //cm
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to B and drop bins off
else {
getPos (coor);
x = coor[0];
dist = xC - x;
driveStraight (dist - 5);
wait1Msec (300);
dropBins();
}
wait1Msec(300);
////////////////////STEP 14/////////////////////
//Go to x coor of home
getPos(coor);
x = coor[0];
dist = xHome - x;
driveStraight(dist);
wait1Msec(300);
///////////////////STEP 15//////////////////////
//Turn north
turnRight(-90);
wait1Msec(300);
///////////////////STEP 16//////////////////////
//Drive to y position
getPos(coor);
y = coor[1];
dist = yHome - y;
driveStraight(dist);
wait1Msec(300);
}
