task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
liftMove(50, 1000);
moveDist(115, 100, SPEED);//Moves forward
turnDeg(45, TURN_RIGHT, 100, SPEED);//Turns right
moveDist(47, 100, SPEED);//Moves forward
}
task main()
{
//wait
servo[topServo] = 235;
initializeRobot();
StartTask(getHeading);
wait1Msec(400);
forwardInc(48, 30);
//Calculates the positioning of the center goal after 10 position tics at 200msec intervals
float a = 0;
float b = 0;
float c = 0;
wait1Msec(400);
StartTask(infared);
wait1Msec(200);
int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a;
int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b;
for(float i = 0; i <= 10; i++){
if(ir1 < ir2-10){
PlayImmediateTone(440, 10);
a += 1;
}else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2
PlayImmediateTone(1024, 10);
b += 1;
}else if(ir1 > ir2+10){
PlayImmediateTone(2024, 10);
c += 1;
}else{
PlayImmediateTone(1024, 10);
//b += 1;
}
wait1Msec(200);
}
ClearSounds();
//code for each of the three center positions a=1 b=2 c=3
StopTask(getHeading);
wait10Msec(50);
StartTask(getHeading);
wait1Msec(200);
if(a > (b+c)/2){
//PlayImmediateTone(440, 10);
currHeading = 0;
turnDeg(-20, 25);
wait10Msec(50);
forwardInc(74, 30);
wait10Msec(50);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr
wait10Msec(25);
forwardInc(-22, 25);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(500);
liftPos(0);
wait10Msec(500);
}else if(b > (a+c)/2){
PlayImmediateTone(1024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(180, 25); //do a one 80
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-36, 30);//go back __ inches
//StartTask(getHeading);
//wait1Msec(100);
//turnDeg(20, 25);
wait10Msec(200);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}else if(c > (a+b)/2){
PlayImmediateTone(2024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-120, 25);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-35, 30);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-32, 25);
forwardInc(-4, 25);
wait10Msec(50);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}
}
void moveTowardAlignedWithObject(int channel, float turnMultiplier, int speed){ //with turnMultiplier set to 0.75, if it expects to need to turn 10 degrees, it will turn 7.5 degrees toward the object.
turnDeg((offsetFromObject(channel, 0)*PIXELS_TO_DEGREES*turnMultiplier), speed); //check for bugs with float multiplication.
//turn to be aligned.
}
