task main()
{
//Wait for the start command//
waitForStart();
//Start getHeading.h//
HTGYROstartCal(GYRO);
StartTask(getHeading);
//Move forward until it sees the beacon or until 5 sec is up//
motor[motorD] = -50;
motor[motorE] = -50;
time1[T3] = 0;
while (atBeacon() == false && time1[T3] < 5000) {
wait1Msec(2);
}
//Turn 90 to the right//
TurnAngle(90, false);
///////////////////////
//Move forward for 1 sec//
motor[motorD] = 80;
motor[motorE] = 80;
wait1Msec(1000);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Move the block belt for 2.5 sec//
motor[motorC] = -100;
wait1Msec(2500);
motor[motorC] = 0;
//Move backward for 1 sec//
motor[motorD] = -50;
motor[motorE] = -50;
wait1Msec(1000);
motor[motorD] = 0;
motor[motorE] = 0;
}
int removePoint(){
printf("Removed point %d %d!\n", pointsList[0][pointsListPointer-1], pointsList[1][pointsListPointer-1]);
pointsListPointer--;
if(pointsListPointer==0){
if(radiusConst!=200){ // if point 0,0 was found with radius 150, end!
return 1;
}
else // if point 0,0 was found with radius 300, switch to radius 100
{
pointsListPointer++;
radiusConst=151;
}
}
//newPointMark();
atBeacon();
getLastPoint();
return 0;
}
bool sab_isPossible() {
int gnd = 1, i;
for(i=0; i < 5; i++)
if(gndVals[i] == 0)
gnd = 0;
if(firstTime && (beaconDir<-20 || beaconDir>20))
return true;
if(firstTime){
atBeacon();
firstTime=0;
}
if(followPoints)
return false;
return gnd;
}