task main()
{

	initializeRobot();

	waitForStart(); // Wait for the beginning of autonomous phase.
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;  // In Inches
float COUNTS_PER_INCH = 90.55;

nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;

wait10Msec(1500);

while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin)
{
	nxtDisplayCenteredTextLine(1, "%d",USreadDist(SONAR_1));
	nxtDisplayCenteredTextLine(2, "%d",SensorValue[IR]);
#if 1
	if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
	{
		motor[L_Motor] = 25;
		motor[R_Motor] = 50;
  }
  else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
  {
  	motor[L_Motor] = 50;
  	motor[R_Motor] = 25;

  }
  else
  {
  	motor[L_Motor] = 50;
		motor[R_Motor] = 50;
	}
#endif
}//End of while.
	motor[L_Motor] = 0;
	motor[R_Motor] = 0;
	wait10Msec(55);

	servo[Block_Chuck] = 255;
	wait10Msec(55);

	servo[Block_Chuck] = 0;
	wait10Msec(55);
while( USreadDist(SONAR_1) < 60)
{
	if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
	{
		motor[L_Motor] = 25;
		motor[R_Motor] = 50;
  }
  else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
  {
  	motor[L_Motor] = 50;
  	motor[R_Motor] = 25;

  }
  else
  {
  	motor[L_Motor] = 50;
		motor[R_Motor] = 50;
	}
}//End of while
	  motor[L_Motor] = 0;
		motor[R_Motor] = 0;
		wait10Msec(55);

		Turn(65);
		wait10Msec(55);

		Move(25,100);
		wait10Msec(55);

		Turn(75);
		wait10Msec(55);

		Move(30 ,100);
		wait10Msec(55);

		servo[Block_Chuck] = 250;
		wait10Msec(55);

		servo[Spring_Release] = 250;
		wait10Msec(55);

		servo[Spring_Release] = 0;
		wait10Msec(55);

		servo[Block_Chuck] = 0;
		wait10Msec(55);

	/* 1. We will need a sweep task.
		 2. Pathfinding move function.
		 		A. Pythagorean theorum to find the sides
		 		B. Try both sides of the obsticle if one side is obstructive.
		 		C. 45 degrees.
	*/
	/*Scan = true;
	while (Scan)
	{
		if (servo[IRS_1] == 0)
		{
			servo[IRS_1] = 255;
		}
		else if (servo[IRS_1] == 255)
		{
			servo[IRS_1] = 0;
		}

  }//End of while*/


} // End main task
Example #2
0
task main()
{
    short right_y;
    short left_y;

    initializeRobot();

    waitForStart();   // wait for start of tele-op phase

    StartTask(endGameTimer);

    displayForward();

    while (true)
    {
        getJoystickSettings(joystick);

        if (!debounce) {
	        if (joy2Btn(RIGHT_TRIGGER_UP)) {
	            handle_event(RIGHT_TRIGGER_UP);
	        } else if (joy2Btn(RIGHT_TRIGGER_DOWN)) {
	            handle_event(RIGHT_TRIGGER_DOWN);
	        } else if (joy2Btn(LEFT_TRIGGER_UP)) {
	            handle_event(LEFT_TRIGGER_UP);
	        } else if (joy2Btn(LEFT_TRIGGER_DOWN)) {
	            handle_event(LEFT_TRIGGER_DOWN);
	        } else if (joy2Btn(BUTTON_ONE)) {
                handle_event(BUTTON_ONE);
            } else if (joy2Btn(BUTTON_FOUR)) {
                handle_event(BUTTON_FOUR);
            } else if (joy1Btn(BUTTON_ONE)) {
                handle_joy1_event(BUTTON_ONE);
            } else if (joy1Btn(BUTTON_FOUR)) {
                handle_joy1_event(BUTTON_FOUR);
            }
        }

        //if (drive_multiplier) {
            //right_y = joystick.joy1_y2;
            //left_y = joystick.joy1_y1;
        //} else {
            right_y = joystick.joy1_y1;
            left_y = joystick.joy1_y2;
        //}

        if (abs(right_y) > 20) {
	    	motor[driveFrontRight] = drive_multiplier * right_y;
	    	motor[driveRearRight] = drive_multiplier * right_y;
		}
		else {
		    motor[driveFrontRight] = 0;
		    motor[driveRearRight] = 0;
		}

        if (abs(left_y) > 20) {
		    motor[driveFrontLeft] = drive_multiplier * left_y;
		    motor[driveRearLeft] = drive_multiplier * left_y;
		}
		else
		{
		    motor[driveFrontLeft] = 0;
		    motor[driveRearLeft] = 0;
		}
    }
}
task main()
{
	initializeRobot();
	waitForStart(); // wait for start of tele-op phase
	while (true)
	{
		getJoystickSettings(joystick);
		//drive code (right or left?)
		if(joystick.joy1_y2 > -15 && joystick.joy1_y2 < 15)//if joysticks are near center power is 0
		{
			motor[leftDrive] = 0;
		}
		else if(joy1Btn(1) == 1){//extra control to go straight forward
			motor[leftDrive] = 50;
		}
		else
		{
			motor[leftDrive] = joystick.joy1_y2*04;//otherwise power is half of the joystick value
		}
		//same for other drive wheels (right or left?)
		if(joystick.joy1_y1 > -15 && joystick.joy1_y1 < 15)
		{
			motor[rightDrive] = 0;
		}
		else if(joy1Btn(1) == 1){//extra control to go straight forward
			motor[rightDrive] = 70;
		}
		else
		{
			motor[rightDrive] = joystick.joy1_y1*0.65;
		}


		//lid controls
		if(joy2Btn(5) == 1){//servo is almost all the way down to help with positioning (prev. 32)
			servo[lidServo] = 223;
		}
		else if(joy2Btn(7) == 1){//
			servo[lidServo] = 200;
		}
		else if(joy2Btn(6) == 1){//lid is shut
			servo[lidServo] = 180;
		}

		//flipper servo controls
		if(joy2Btn(1) == 1){
			servo[flipperServo] = 255;//flips the box over to score in the center goal.
		}
		else if(joy2Btn(3) == 1){
			servo[flipperServo] = 0;//moves the box so it is on the flat of the gear, so it can be folded into the arm.
		}
		else if(joy2Btn(2) == 1){
			servo[flipperServo] = 100;//moves the box in position to score in the highest rolling goal.

		}

		//Harvester controls (button 6 is high speed, button 8 is low speed)
		if(joy1Btn(6) == 1)//harvester only runs when arm is down
		{
			motor[harvester] = 40;
		}
		else if(joy1Btn(8) == 1)//reverses harvester
		{
			motor[harvester] = -40;
		}
		else
		{
			motor[harvester] = 0;
		}

		//arm controls (btn 10 up, btn 9 down)

		if(joy2Btn(10) == 1 && getArmPosition() != ARM_EXTENDED)//when the arm isn't up it can go up
		{
			motor[arm] = 100;
		}
		else if(joy2Btn(9) == 1 && getArmPosition() != ARM_FOLDED)//when arm isn't down it can go down
		{
			motor[arm] = -80;
		}
		else
		{
			motor[arm] = 0;
		}

		//goal grabber controls
		if(joy1Btn(7) == 1){
			servo[goalServo] = 100;
		}
		else if(joy1Btn(5) == 1){
			servo[goalServo] = 64;
		}
		//else{

		//}
	}//end while
}
task main()
{
  initializeRobot();
	int countline = 0;
  waitForStart(); // Wait for the beginning of autonomous phase.
  wait1Msec(10000);
  motor[slide] = -20;//slide over
	wait1Msec(750);
	motor[slide] = 0;
	ClearTimer(T1);
	SensorValue[irSeek] = 0;
	motor[leftDrive]  = 70;
	motor[rightDrive] = 70;
	servo[rightgrab] = grabDownPosition;//set to start position
  while(time1[T1] < 9000)
	{
		if(atLine())
		{
			countline++;

			if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6)//checks if at the ir beacon
			{
				// at Correct Line
				break;
			}
			else if(countline==1)// if hit the 1st line turn slightly
			{
				motor[rightDrive]= 0;
				motor[leftDrive] = 9;
				wait1Msec(500);
			}
			else if(countline == 2){//if hit the 2nd line turn slightly
				motor[rightDrive] = 0;
				motor[leftDrive] = 1;
				wait1Msec(500);
			}
			motor[leftDrive]  = 75;//drive
			motor[rightDrive] = 75;

			wait1Msec(500);

		}
	}
	if(time1[T1] < 9000)//while less thatn 9 seconds
	{
		nMotorEncoder[leftDrive]  = 0;
		nMotorEncoder[rightDrive] = 0;
		if(countline == 1){

			forward(8.9); //Distance after line till stop
			stopDrive();
			wait1Msec(500);

			servo[rightgrab] = grabReleasePositionpeg1;//put grabber in position to place ring
			wait1Msec(500);
			motor[slide] = 40;//move slide over
			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
	    servo[rightgrab] = grabReleasePositionpeg1 + 10;//move slightly to help put ring on
	    wait1Msec(1000);
			forward(7);//go forward to pull ring off
		}
		else if(countline == 2)
		{
			forward(8.1);//Distance after line till stop
			stopDrive();
			wait1Msec(500);

			servo[rightgrab] = grabReleasePositionpeg2;//put grabber in position to place ring
			wait1Msec(500);
			motor[slide] = 40;//move over to put ring on
			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
	    servo[rightgrab] = grabReleasePositionpeg2 -8;//move slightly to put ring on
	    wait1Msec(1000);
			forward(7);//go forward to pull ring off
		}
		else if(countline == 3){
			forward(8.97);//Distance after line till stop
			stopDrive();
			wait1Msec(500);

			servo[rightgrab] = grabReleasePositionpeg3;//move grabber to position to place ring on 3 peg
			wait1Msec(500);
			motor[slide] = 40;//move over to place ring on
			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
	    servo[rightgrab] = grabReleasePositionpeg3 +5;//move slightly to help put ring on
	    wait1Msec(1000);
			forward(7);//go forward to pull ring off
		}






	}
	else
		stopDrive();//if not less than 9 seconds stop
	nMotorEncoder[leftDrive]  = 0;//clea encoders
	nMotorEncoder[rightDrive] = 0;
  while (true) {}
}
Example #5
0
task main()
{
    // The IR signal strengh in all 5 directions.
    int IRdirA = 0;
    int IRdirB = 0;
    int IRdirC = 0;
    int IRdirD = 0;
    int IRdirE = 0;



    waitForStart();

    initializeRobot();



    // The amount of time the robot...
    const int forwardTimeAA	= 25;
    const int turnTimeA 	= 50;

    const int forwardTimeA 	= 170;
    const int turnTimeB 	= 110;
    const int forwardTimeB 	= 100;
    const int liftTimeB 	= 45;

    const int forwardTimeCA	= 110;	//TODO
    const int forwardTimeCB = 40;	//TODO
    const int turnTimeD 	= 152;
    const int forwardTimeD 	= 110;
    const int liftTimeD 	= 135;

    const int forwardTimeE 	= 95;	//TODO
    const int turnTimeF 	= 112;
    const int forwardTimeF 	= 80;
    const int liftTimeF 	= 47;

    const int liftTimeG		= 30;	//TODO
    const int backwardTimeG	= 100;	//TODO
    const int turnTimeG		= 70;	//TODO
    const int forwardTimeG	= 20;	//TODO

    const int liftTimeH		= 50;	//TODO
    const int backwardTimeH	= 90;	//TODO
    const int turnTimeH		= 100;	//TODO
    const int forwardTimeH	= 70;	//TODO

    const int liftTimeI		= 30;	//TODO
    const int backwardTimeI	= 130;	//TODO
    const int turnTimeI		= 70;	//TODO
    const int forwardTimeI	= 170;	//TODO

    const int forwardTimeJ	= 50;	//TODO
    const int turnTimeK		= 90;	//TODO
    const int liftTimeK		= 30;	//TODO
    const int forwardTimeK	= 50;	//TODO



    Move_Forward	(forwardTimeAA, g_AccurateMotorPower);
    Turn_Left		(turnTimeA, g_AccurateMotorPower, g_AccurateMotorPower);
    Move_Forward	(forwardTimeA, g_AccurateMotorPower);

    Time_Wait(50);
    HTIRS2readAllDCStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);

    if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
    {
        Turn_Right		(turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower);
        Lift_Up			(liftTimeB, g_AccurateMotorPower);
        Move_Forward	(forwardTimeB, g_AccurateMotorPower);

        Lift_Down		(liftTimeG, g_AccurateMotorPower);
        Move_Backward	(backwardTimeG, g_AccurateMotorPower);
        Turn_Right		(turnTimeG, g_AccurateMotorPower, g_AccurateMotorPower);

        Move_Forward	(forwardTimeG, g_AccurateMotorPower);
    }
    else
    {
        Move_Forward	(forwardTimeCA, g_AccurateMotorPower);
        Time_Wait(50);
        HTIRS2readAllACStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
        Move_Forward	(forwardTimeCB, g_AccurateMotorPower);

        if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold )
        {
            Turn_Right		(turnTimeD, g_AccurateMotorPower, g_AccurateMotorPower);
            Lift_Up			(liftTimeD, g_AccurateMotorPower);
            Move_Forward	(forwardTimeD, g_AccurateMotorPower);

            Lift_Down		(liftTimeH, g_AccurateMotorPower);
            Move_Backward	(backwardTimeH, g_AccurateMotorPower);
            Turn_Right		(turnTimeH, g_AccurateMotorPower, g_AccurateMotorPower);

            Move_Forward	(forwardTimeH, g_AccurateMotorPower);
        }
        else
        {
            Move_Forward	(forwardTimeE, g_AccurateMotorPower);

            Turn_Right		(turnTimeF, g_AccurateMotorPower, g_AccurateMotorPower);
            Lift_Up			(liftTimeF, g_AccurateMotorPower);
            Move_Forward	(forwardTimeF, g_AccurateMotorPower);

            Lift_Down		(liftTimeI, g_AccurateMotorPower);
            Move_Backward	(backwardTimeI, g_AccurateMotorPower);
            Turn_Right		(turnTimeI, g_AccurateMotorPower, g_AccurateMotorPower);

            Move_Forward	(forwardTimeI, g_AccurateMotorPower);
        }
    }
    Move_Forward	(forwardTimeJ, g_AccurateMotorPower);
    Turn_Right		(turnTimeK, g_AccurateMotorPower, g_AccurateMotorPower);
    Lift_Up			(liftTimeK, g_AccurateMotorPower);
    Move_Forward	(forwardTimeK, g_AccurateMotorPower);



    while (true)
    {
        PlaySoundFile("moo.rso");
        while(bSoundActive == true)
        {
            Time_Wait(1);
        }
    }
}
task main()
{
	initializeRobot();

	waitForStart(); // Wait for the beginning of autonomous phase.
	int IRvalue = SensorValue[IR];
	wait10Msec(140);

	IRvalue = 7;

	if(IRvalue < 5 || IRvalue > 7)
	{
		IRvalue = 0;
	}
//Robot takes reading of the IR


	if(IRvalue==5)//Far
	{
		wait10Msec(25);

  	Move(52,75);
		wait10Msec(55);

		Turn(42);
		wait10Msec(55);

		Arm_move(149);
		wait10Msec(55);

		motor[Lift] = 75;
		wait10Msec(25);
		motor[Lift] = 0;

		Move(28.5,75);
		wait10Msec(55);

		Arm_move(1);
		wait10Msec(55);

		Move(-1,75);
		wait10Msec(55);

		Arm_move(1);
		wait10Msec(55);

		Move(-1,75);
		wait10Msec(55);

		Arm_move(3);
		wait10Msec(55);

		Move(-15,75);
		wait10Msec(55);

	}

	if(IRvalue==6 || IRvalue==0)//Middle
	{
         wait10Msec (55);
		Move(27,75);
		wait10Msec(55);


		Turn(51);
		wait10Msec(55);

		Arm_move(154);
		wait10Msec(55);

		motor[Lift] = 75;
		wait10Msec(32);
		motor[Lift] = 0;

		Move(42.5,80);
		wait10Msec(55);

		Arm_move(12);
		wait10Msec(55);

		Move(-15,75);
		wait10Msec(55);

  	Turn(50);
  	wait10Msec(55);

  	Move(-10,75);
  	wait10Msec(55);

	}

	if(IRvalue==7)//Close
	{
		wait10Msec(55);
		Move(4,60);
		wait10Msec(55);

		Turn(80);
		wait10Msec(55);

		Move(25,75);
		wait10Msec(55);

		Turn(-36);
		wait10Msec(55);

		Arm_move(149);
		wait10Msec(55);

		motor[Lift] = 75;
		wait10Msec(25);
		motor[Lift] = 0;

		Move(26,75);
		wait10Msec(10);

		Arm_move(22);
		wait10Msec(55);

		Move(-7,75);
		wait10Msec(55);


	}



}
// the starting point of the program
task main()
{
    // prompt the user for a delay at the start of autonomous
    selectTime();
    // prompt whether the robot should wait for FCS to start
    // They can disable it when testing the program at practice.
    startSelect();
    // wait for FCS to start if the user has chosen to
    if(waitSelected)
    {
        waitForStart();
    }
    // perform the aforementioned delay
    wait1Msec(1000*waitTime);
    //holds which beacon the robot is at
    int beacon = 0;
    //holds the IR sensor value
    int i = 0;

    //loops until the sensor finds the beacon or until it gets to the final beacon.
    while(i != 5 && beacon < 4)
    {
        i=0;
        //this section prevents the reading of random IR values and makes sure the value is constant.
        //when the value is the constant we want, the beacon is in front of the sensor.
        for(int a = 0; a <100; a++)
        {
            if(a == 0)
            {
                //gets original ir value
                i  = SensorValue[IR];
            }
            else
            {
                int b = SensorValue[IR];
                if(b != i)
                {
                    //if a different value is sensed, then the original value was not constant; thus, we restart the checking
                    //to try and gete a constant value.
                    i = 0;
                    a = 0;
                }
            }
        }
        if(i != 5)
        {
            if(beacon == 0)
            {
                //the distance to get to the first beacon
                ModForward(2200,35);
                motor[Right] = 0;
                motor[Left] = 0;
                wait1Msec(1000);
            }
            else if(beacon == 1)
            {
                //the distance to get to the second beacon
                ModForward(900,35);
                motor[Right] = 0;
                motor[Left] = 0;
                wait1Msec(1000);
            }
            else if(beacon == 2)
            {
                //the distance to get to the third beacon
                ModForward(1600,35);
                motor[Right] = 0;
                motor[Left] = 0;
                wait1Msec(1000);
            }
            else if(beacon == 3)
            {
                //the distance to get to the fourth beacon
                ModForward(1000,35);
                motor[Right] = 0;
                motor[Left] = 0;
                wait1Msec(1000);
            }
            beacon++;
        }
    }

    //code that runs after  we stop in front of a beacon.
    servo[AutoHook] = 255;	//dispenses the cube
    wait1Msec(900);
    servo[AutoHook] = 0;
    wait1Msec(1000);

    //pick where to  go based on beacon #.
    //if the robot is at rings 1 or 4, it turns, goes back, then turns to face the ramp.
    //if the robots is at rings 2 or 3, it goes backwards, turns, goes backwards again, and then turns to face the ramp.
    if(beacon == 1) //beacon was on first box
    {
        //goes backwards from the box
        ModBackward(1760,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns to be parallel to the ramp
        ModTurn(1840, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes backwards and towards the ramp
        ModBackward(4000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns towards the ramp
        ModTurn(2100, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes onto the ramp
        ModBackward(4500,100);
    }
    else if (beacon == 2) //beacon was on second box
    {
        //goes backwards from the box
        ModBackward(3400,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns to be parallel to the ramp
        ModTurn(1840, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes backwards and towards the ramp
        ModBackward(4000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns towards the ramp
        ModTurn(2100, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes onto the ramp
        ModBackward(4500,100);
    }
    else if (beacon == 3) //beacon was on third box
    {
        //goes backwards from the box
        ModBackward(5000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns to be parallel to the ramp
        ModTurn(1840, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes backwards and towards the ramp
        ModBackward(4000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns towards the ramp
        ModTurn(2100, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes onto the ramp
        ModBackward(4500,100);
    }
    else if (beacon == 4) //beacon was on fourth ring
    {
        //goes backwards from the box
        ModBackward(6000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns to be parallel to the ramp
        ModTurn(1840, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes backwards and towards the ramp
        ModBackward(4000,50);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //turns towards the ramp
        ModTurn(2100, 100, right);
        motor[Right] = 0;
        motor[Left] = 0;
        wait1Msec(500);

        //goes onto the ramp
        ModBackward(4500,100);
    }
}
Example #8
0
//=============================================================================================================================================
//---------------------------------------------------BEGIN INITIALIZATION CODE-----------------------------------------------------------------
task main() {

	//Initialize the display with the program choices
	chooseProgram();

	switch (PROGID) {
		case 1:
			FORWARD_SCORE_FORWARD_LINE_1 = true;
			linesToFind = 1;
			break;
		case 2:
			FORWARD_SCORE_FORWARD_LINE_2 = true;
			linesToFind = 2;
			break;
		case 3:
			FORWARD_SCORE_BACKWARD_LINE_1 = true;
			linesToFind = 1;
			break;
		case 4:
			FORWARD_SCORE_BACKWARD_LINE_2 = true;
			linesToFind = 2;
			break;
		case 5:
			useDummyAutonomous();
			break;
		case 6:
			//useOriginalAutonomous();
			PlaySoundFile("Woops.rso");
			break;
	}

//---------------------------------------------------------END INITIALIZATION CODE-------------------------------------------------------------
//=============================================================================================================================================

	//if (PROGID == 1 || PROGID == 2 || PROGID == 3 || PROGID == 4) {

	TFileHandle irFileHandle;
	TFileIOResult IOResult;
	HTGYROstartCal(HTGYRO);
	//PlaySound(soundBlip);
	//wait1Msec((2 * PI) * 1000); //TAUUUU
	//wait1Msec(10000);//wait 10 seconds for other teams who **might** have better autonomous code
	PlaySound(soundFastUpwardTones);

	//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
	//=================================================================================================
	int _dirDCL = 0;
	int _dirACL = 0;
	int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
	int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
	int _dirEnhL, _strEnhL;

	// the default DSP mode is 1200 Hz.

	initializeRobot();
	servo[servoLift] = 123;
	servo[servoSweep] = 199;
	waitForStart();
	ClearTimer(T1);
	OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);

	// FULLY DYNAMIC CODE W/ SCORING OF CUBE
	while(searching)
	{

		//float irval = acS3L;
		//StringFormat(irvalres, "%3.0f", irval);

		//WriteText(irFileHandle, IOResult, "Test");
		//WriteString(irFileHandle, IOResult, irvalres);
		//WriteByte(irFileHandle, IOResult, 13);
		//WriteByte(irFileHandle, IOResult, 10);


		_dirDCL = HTIRS2readDCDir(HTIRS2L);
		if (_dirDCL < 0)
			break; // I2C read error occurred

		_dirACL = HTIRS2readACDir(HTIRS2L);
		if (_dirACL < 0)
			break; // I2C read error occurred

		//===========LEFT SIDE
		// Read the individual signal strengths of the internal sensors
		// Do this for both unmodulated (DC) and modulated signals (AC)
		if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
			break; // I2C read error occurred
		if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
			break; // I2C read error occurred

		//=================Enhanced IR Values (Left and Right)===========
		// Read the Enhanced direction and strength
		if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
			break; // I2C read error occurred

		//______________END SENSORVAL BLOCK________________________________________________________________
		//=================================================================================================

		if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
			motor[motorLeft] = -80;
			motor[motorRight] = -80;

			if (time1[T1] > timeToEnd) {
				searching = false;
				koth = true;
				goToEnd = false;
				//if it doesnt find the beacon, dont bother returning to start if it has been set to
			}
		}

		//===================================BLOCK FOR IR DETECTION=====================
		if (acS3L > irFindVal) { //if sensor is directly in front of beacon

			if (time1[T1] < 2000) {
				wait1Msec(600);
			}

			motor[motorLeft] = 0;
			motor[motorRight] = 0;
			//irOnLeft = true;
			searching = false;
			koth = true;
			goToEnd = true;
		}

		//==================END IR DETECTION BLOCK========================

		wait1Msec(30);
	}//while searching

	//Close(irFileHandle, IOResult);
	roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer

	scoreCube();

	if (goToEnd) {
		if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
			driveToEnd(-80, timeToEnd - roundTime);//drive to end of ramp
		}
		if (FORWARD_SCORE_BACKWARD_LINE_1 || FORWARD_SCORE_BACKWARD_LINE_2) {
			driveToEnd(80, roundTime);
		}
	}

	wait1Msec(300);
//=======================================================================================================================================
//------------------------BEGIN 90 DEGREE TURNS------------------------------------------------------------------------------------------

	//HTGYROstartCal(HTGYRO);
	ClearTimer(T3);

	while (true) {

		wait1Msec(20);

		//if (abs(rotSpeed) > 3) {
		rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
		heading += (rotSpeed * 0.02);//find gyro heading in degrees??

		motor[motorLeft] = 60;
		motor[motorRight] = -60;

		if (heading <= degFirstTurn) {

			motor[motorLeft] = 0;
			motor[motorRight] = 0;
			//---------------LINE DETECTOR--------------------------
			LSsetActive(LEGOLS);
			while (linesFound < linesToFind) {
				motor[motorLeft] = -50;
				motor[motorRight] = -50;
				wait1Msec(10);
				if (LSvalNorm(LEGOLS) >= WHITE_LINE_VALUE) {
					linesFound++;
				}
				if (linesFound >= linesToFind) { //ever-present failsafe
					break;
					LSsetInactive(LEGOLS);
				}
			}

			if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
				while (true) {
					wait1Msec(20);
					rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
					heading += (rotSpeed * 0.02);//find gyro heading in degrees??

					motor[motorLeft] = 60;
					motor[motorRight] = -60;

					if (heading <= degSecondTurn) {
						motor[motorLeft] = 0;
						motor[motorRight] = 0;

						moveForward(3.3, 100);
						break;
					}
				}
			} else {
				while (true) {
					wait1Msec(20);
					rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
					heading += (rotSpeed * 0.02);//find gyro heading in degrees??

					motor[motorLeft] = -60;
					motor[motorRight] = 60;

					if (heading <= 92) {
						motor[motorLeft] = 0;
						motor[motorRight] = 0;

						moveForward(3.3, 100);
						break;
					}
				}
			}
			break;
		}
	}

//==================================================================================

	//Begin KotH routine
	servo[servoUSSeeker] = 92;
	USScanVal = 92;
	float heading = 92;
	HTGYROstartCal(HTGYRO);

	while (koth) {

		//wait1Msec(1000);
		//SCAN LEFT==========================
		while(true) {

			servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
			USScanVal += 5;
			wait1Msec(100);

			if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
				PlaySound(soundFastUpwardTones);
				searchingForBot = true;
				turnedLeft = true;
				turnedRight = false;
				turnTowardsRobot();
				pushOffRamp();
				turnTowardsCenter();
			}
			if (USScanVal > 135) {
				break;
			}
		}
		//SCAN RIGHT=========================
		while(true) {
			servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
			USScanVal -= 5;
			wait1Msec(100);
			if (USScanVal < 40) {
				break;
			}
			if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
				PlaySound(soundFastUpwardTones);
				searchingForBot = true;
				turnedLeft = false;
				turnedRight = true;
				turnTowardsRobot();
				pushOffRamp();
				turnTowardsCenter();
			}
		}

		if (nPgmTime > 29000) {
			koth = false;
		}
	}//while koth
	MissionImpossible();
/*
}//END MAIN IF PROGIDS THING

else if (PROGID == 5) {

	useDummyAutonomous();

}
*/

}//task main
Example #9
0
task main()
{
	int threshold = 40;
	/*//removed for testing

	servo[Winch]=127;
	*/
	//
	//  going straight
	//
	int straight_FRS = 136;
	int straight_BRS = 146;
	int straight_FLS = 132;
	int straight_BLS = 134;



	//  mandible servo positions
	//
	int openwideR = 142;
	int openwideL = 100;
	int shutR = 0;
	int shutL = 228;
	//  mouth
	int mouthup=60;
	int mouthdown=255;
	//
	int elevatorposition=0;
	int uppositionscore=62;
	int downpositionscore= 5;
	// gate
	int gateup = 5;
	int gatedrive = 50;
	int gatedown = 202;
	//
	int lockup = 0;
	int lockdown = 85;
	//
	int scoreopen = 5;
	int scoreclose = 245;

	int FRS =0;
	int BRS = 0;
	int FLS  = 0;
	int BLS = 0;
	//    servo initialization
	servo[leftmandible] = openwideL;
	servo[rightmandible] = openwideR;
	//	servo[mouth] = mouthup;
	servo[score] = scoreclose;
	servo[lock] = lockup;
	servo[gate] = gateup;
	//
	//  wheels  go straight to start
	//
	servo[frontRS]= straight_FRS;
	servo[backRS]= straight_BRS;
	servo[frontLS]= straight_FLS;
	servo[backLS]= straight_BLS;

	int lastservopos = 0;   // 1 = straight, 2 = spin , 3 = turn 45 deg to right, 4 = turn 45degrees to left, 5 = turn wheels to side

	waitForStart();
//turnServos(1, lastservopos);
servo[gate] = gatedrive;

	while(true)
	{
		getJoystickSettings(joystick); //Constantly updates values on the joystick
		//  left joystick

		if((abs(joystick.joy1_y1) > threshold)|| (abs(joystick.joy1_x1) > threshold) || (abs(joystick.joy1_x2) > threshold))
		{
			if ((abs(joystick.joy1_y1) > threshold) && (abs (joystick.joy1_x1) > threshold))   // going 45 degrees
			{
				BRS = FRS = BLS = FLS = (abs(joystick.joy1_y1) * 100)/joystick.joy1_y1;  // TAKE POWER AND DIRECTION from Y axis
				if (((joystick.joy1_y1 * joystick.joy1_x1)) > 0)			// if both positive or both negative go to right
				{
					turnServos(3, lastservopos);  // turn wheels to right to go 45 degrees
					lastservopos = 3;

				} else
				{
					turnServos(4, lastservopos);  // turn wheels to left to go 45 degrees
					lastservopos = 4;
				}
			} else if (((abs(joystick.joy1_y1) > threshold)) &&(abs(joystick.joy1_x1) <= threshold))  // go straight

			// if going forward or back
			{
				BRS = FRS = BLS = FLS = (abs(joystick.joy1_y1)*100)/joystick.joy1_y1;
				// servos
				turnServos(1, lastservopos);
				lastservopos = 1;
				// motors

			} else if ((abs(joystick.joy1_x1) > threshold) &&  (abs(joystick.joy1_y1) <= threshold)) // go sideways
			{
				BRS = FRS = BLS = FLS = (abs(joystick.joy1_x1)*100)/joystick.joy1_x1;  //  positive = go right, negative = go left.
				// servos
				turnServos(5, lastservopos);  // turn wheels to right to go sideways
				lastservopos = 5;
				// motors

			} else if (abs(joystick.joy1_x2) > threshold) // spin
			{
				FRS = BRS = -(abs(joystick.joy1_x2)*100)/joystick.joy1_x2;  //  positive = go right, negative = go left.
				BLS = FLS = -FRS;   // have to switch direction to make it spin.
				// servos
				turnServos(2, lastservopos);  // turn wheels to right to go sideways
				lastservopos = 2;
			}
		}else
		{
			BRS = FRS = BLS = FLS = 0;
		}

		// Now tell motors to go
		motor[RightFront] = FRS;
		motor[LeftFront] = FLS;
		motor[RightBack] = BRS;
		motor[LeftBack] = BLS;

		// Mandible controls
		if(joy1Btn(8))
		{

			servo[rightmandible] = shutR;
			servo[leftmandible] = shutL;
		}
		if(joy1Btn(6))
		{
			servo[rightmandible] = openwideR;
			servo[leftmandible] = openwideL;
		}
		/*
		// Mouth controls

		if(joy1Btn(4))
		{
		servo[mouth]=mouthup;
		wait1Msec(30);

		servo[rightmandible] = openwideR;  // open mandibles so they don't hit elevator
		servo[leftmandible] = openwideL;
		}
		if(joy1Btn(2))
		{
		servo[mouth]=mouthdown;
		}

		/////////////    JOYSTICK 2 //////////////////

		//  elevator controls

		if(abs(joystick.joy2_y2) > threshold)// absolute value so can be negative or positive
		{
		motor[elevator] = 100*joystick.joy2_y2/abs(joystick.joy2_y2);//Raises the elevator
		}
		else
		{
		motor[elevator] = 0;
		}
		*/
		// scoring

		if(joy2Btn(8))
		{

			servo[score] = scoreopen;
		}
		else
		{
			servo[score]=scoreclose;
		}

		// rolling goal

		if(joy2Btn(2))   // gate down to capture rolling goal
		{

			servo[gate] = gatedown;
		}

		if (joy2Btn(4))		// gate up
		{
			servo[lock] = lockup;
			wait1Msec(5);
			servo[gate] = gatedrive;

		}

		if (joy2Btn(3))		// lock on
		{
			servo[lock] = lockdown;
		}

		if (joy2Btn(1))     // lock up - release rolling goal
		{
			servo[lock] = lockup;
		}


	}
}
Example #10
0
task main()
{
  motor[lift] = 0;
	waitForStart();
	//irSeeker.acDirection;
	initializeRobot();
  //int centerGoal;

	moveBackward(7);
	stopBot();
	/*wait10Msec(150);
	turnLeft(105);
	stopBot();
	wait10Msec(100);
  moveBackward(2.45);
  stopBot();
  wait10Msec(100);
	readSensor(&irSeeker);
	centerGoal = irSeeker.acDirection;
  //IRseeker();
  displayTextLine(1, "D:%4d", irSeeker.acDirection);

	if(centerGoal == 0)
	{
		stopBot();
	}
	else if(centerGoal <= 3 && centerGoal > 0)
	{
	 playSound(soundBeepBeep);
	 moveForward(1);
	 wait10Msec(50);
	 turnLeft(100);
	 wait10Msec(50);
	 moveBackward(3.3);
	 wait10Msec(50);
	 turnRight(80);
	 wait10Msec(50);
	 moveBackward(2);
	 wait10Msec(50);


  }
  else if(centerGoal >= 3 && centerGoal < 5)
  {
   playSound(soundDownwardTones);
   moveForward(1);
	 wait10Msec(50);
	 turnLeft(100);
	 wait10Msec(50);
	 moveBackward(2);
	 wait10Msec(50);
	 turnRight(70);
	 wait10Msec(50);
	 moveBackward(1.5);
	 wait10Msec(50);
  }
  else if(centerGoal >= 5 && centerGoal <= 9)
  {
   playSound(soundFastUpwardTones);
   moveForward(.75);
	 wait10Msec(50);
	 turnLeft(90);
	 wait10Msec(50);
	 moveBackward(1.5);
	 wait10Msec(50);
	 turnRight(80);
	 wait10Msec(50);
	 moveBackward(2);
	 wait10Msec(50);

  }
  else
  {
  	stopBot();
  }

  wait10Msec(1000);

*/



}
Example #11
0
task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  // Initialize variables
  int gear = 1;
  float turnFactor;

  while (true)
  {
	  ///////////////////////////////////////////////////////////
	  ///////////////////////////////////////////////////////////
	  ////                                                   ////
	  ////      Add your robot specific tele-op code here.   ////
	  ////                                                   ////
	  ///////////////////////////////////////////////////////////
	  ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.

    getJoystickSettings(joystick);

    // Determine how much to slow down one motor to turn
    turnFactor = abs(joystick.joy1_x1) / 127;

    if (joy1Btn(7) == 1)

    {

      // Moving forward

      if (joystick.joy1_x1 > 5) {

        // Turn right
        motor[leftMotor] = gear * 20;
        motor[rightMotor] = gear * 20 * turnFactor;

      } else {

        if (joystick.joy1_x1 < -5) {
          // Turn left
	        motor[leftMotor] = gear * 20 * turnFactor;
	        motor[rightMotor] = gear * 20;

	      } else {

        // Move straight
	        motor[leftMotor] = gear * 20;
	        motor[rightMotor] = gear * 20;

        }

      }

    } else {

      // Stop
      motor[leftMotor] = 0;
      motor[rightMotor] = 0;

    }

  }

}
task main()
{
	waitForStart();
	moveForward(6.5, 80);
	wait10Msec(50);
	stopMotors();
	wait10Msec(30);
	leftTwoWheelTurn(45, 50);
	wait10Msec(50);
	stopMotors();
	wait10Msec(30);
	motor[tiltingMotor] = 75;
	//robot going to score
	wait10Msec(70);
	motor[tiltingMotor] = 25;
	wait10Msec(5);
	motor[tiltingMotor] = 0;
	wait10Msec(10);
	//robot ready to roll out blocks
	motor[conveyorMotor] = 100;
	wait10Msec(20);
	motor[conveyorMotor] = 0;
	wait10Msec(20);
	//robot done scoring
	motor[tiltingMotor] = -50;
	wait10Msec(57);
	motor[tiltingMotor] = -25;
	wait10Msec(10);
	motor[tiltingMotor] = 0;
	wait10Msec(4);
	//robot's tilting motor comes back to normal state
	moveBackward(2, 80);
	wait10Msec(50);
	//robot moves backward, so it will be able to sense the ir beacon underneath first bucket
	stopMotors();
	wait10Msec(30);
	//keep going forward until ir sensor senses ir beacon
	StartTask(irLeftTesting);
	wait10Msec(800);
	//wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum
	while (SensorValue[sonarSensor] < 75)
	{
		motor[frontLeftMotor] = 100;
		motor[frontRightMotor] = 100;
		motor[backRightMotor] = 100;
		motor[backLeftMotor] = 100;
	}
	stopMotors();
	wait10Msec(30);
	moveForward(8, 80);
	wait10Msec(50);
	rightTwoWheelTurn(48, 50);
	wait10Msec(60);
	moveForward(25, 80);
	wait10Msec(50);
	rightTwoWheelTurn(48, 50);
	wait10Msec(58);
	moveForward(28.5, 80);
	wait10Msec(50);
	leftTwoWheelTurn(53, 50);
	wait10Msec(120);
	moveBackward(43.5, 80);
	wait10Msec(50);
	//robot is parked in the middle of the ramp
}
Example #13
0
task main()
{
	// These will be used later and are declared here to save from having to
	// declare them every single loop.
	int powerL = 0;
	int powerR = 0;
	int powerPopcorn = 0;
	MotorState isMotorStateL = MOTOR_JOYSTICK;
	MotorState isMotorStateR = MOTOR_JOYSTICK;



	waitForStart();

	initializeRobot();



	while (true)
	{
		// Currently does (at least) 7 checks and 3 assignments per loop.
		Joystick_UpdateData();



		// These should be zeroed after every loop. In the case that there
		// isn't input, the motors won't keep moving at the last speed it had.
		powerL = 0;
		powerR = 0;
		powerLift = 0;
		powerPopcorn = 0;

		// POPCORN!!! (This comes first, obviously.)
		if ( Joystick_Button(BUTTON_B, CONTROLLER_2)==true )
		{
			powerPopcorn = g_FullDrivePower;
		}
		else if ( Joystick_Button(BUTTON_A, CONTROLLER_2)==true )
		{
			powerPopcorn = (-1)*g_FullDrivePower;
		}



		// See if a direction is being pressed, then test for the direction.
		// This is inside an `if` statement to optimize speed (less checking).
		// `JoystickController` arguments are not passed to increase speed.

		// Input from CONTROLLER_2 will be used to control the lift in
		// conjunction with CONTROLLER_1, but shouldn't override the driver,
		// since driver #1's input is processed last.

		// This is the code for CONTROLLER_2:
		if ( abs(joystick.joy2_y1)>g_JoystickThreshold )
		{
			isLiftState = LIFT_JOYSTICK;
			//powerLift = Math_ToLogarithmic(joystick.joy2_y1);
			powerLift = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y, CONTROLLER_2));
		}
		if ( (	Joystick_Button(BUTTON_LB, CONTROLLER_2) ||
				Joystick_Button(BUTTON_RB, CONTROLLER_2)) ==true )
		{
			isLiftState = LIFT_JOYSTICK;
			powerLift /= g_FineTuneFactor;
		}

		// This is the code for CONTROLLER_1, along with two unimplemented
		// functions for putting rings on and taking rings off.
		if ( Joystick_Direction() != DIRECTION_NONE )
		{
			switch ( Joystick_Direction() )
			{

				// Operate lift at full power if F/B.
				case DIRECTION_F:
					isLiftState = LIFT_JOYSTICK;
					powerLift = g_FullLiftPower;
					break;
				case DIRECTION_B:
					isLiftState = LIFT_JOYSTICK;
					powerLift = (-1)*g_FullLiftPower;
					break;

				case DIRECTION_L:
					sub_PutRingOn();
					break;
				case DIRECTION_R:
					StartTask(sub_TakeRingOff);
					break;
			}
		}



		// See if a button (not masked) is being pressed, then react.
		// This is inside an `if` statement to optimize speed (less checking).

		// The argument to this first `if` statement is a masked version
		// of the "bitmap" of buttons directly from the controller.

		// Everything other than the buttons used are masked off, to increase
		// processing speed (possibly, just speculation). Reasoning:
		// `&` compares all bits of the variables, so we might as well mask
		// everything we won't need, in case something irrelevant is pressed.

		// A `0` value means no buttons (that we are testing for) are pressed.
		// Directly using the struct since this is the only possible time to
		// use it, and this is very low-level anyways.

		//if ( joystick.joy1_Buttons != false )
		//if ( (g_ControllerMaskA & joystick.joy1_Buttons) != false )
		{

			// Buttons Y/B/A will control lift height.
			if ( Joystick_Button(BUTTON_Y)==true )
			{
				isLiftState = LIFT_TOP;
			}
			if ( Joystick_Button(BUTTON_B)==true )
			{
				isLiftState = LIFT_MIDDLE;
			}
			if ( Joystick_Button(BUTTON_A)==true )
			{
				isLiftState = LIFT_BOTTOM;
			}

			// If only X is pressed, weigh the ring.
			// If JOYR is pressed as well, deploy ramp.
			if ( Joystick_Button(BUTTON_X)==true )
			{
				if ( Joystick_Button(BUTTON_JOYR) == true )
				{
					Servo_Rotate(servo_ramp, g_rampServoDeployed);
				}
				else
				{
					StartTask(sub_WeighRings);
				}
			}

			// Buttons LT/RT will fine-tune the lift.
			if ( Joystick_Button(BUTTON_RT)==true )
			{
				isLiftState = LIFT_JOYSTICK;
				powerLift = g_FullLiftPower/g_FineTuneFactor;
			}
			if ( Joystick_Button(BUTTON_LT)==true )
			{
				isLiftState = LIFT_JOYSTICK;
				powerLift = (-1)*g_FullLiftPower/g_FineTuneFactor;
			}

		}



		// L/R motor code. Only triggered when a joystick returns a
		// value greater than the "drive" threshold (`global vars.h`).

		// Logarithmic control probably won't be implemented anytime soon.
		// Also need to stop using the `joystick` struct and switch to the
		// encapsulated version (Joystick_Joystick(...)).



		// Y-axis code:
		if ( 	abs(Joystick_Joystick(JOYSTICK_L, AXIS_Y)) > g_JoystickThreshold ||
				abs(Joystick_Joystick(JOYSTICK_R, AXIS_Y)) > g_JoystickThreshold )
		{
			powerL = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y));
			powerR = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_R, AXIS_Y));
		}

		// Last check: if LB/RB is pressed, fine-tune the power level.
		if ( (Joystick_Button(BUTTON_LB)||Joystick_Button(BUTTON_RB)) ==true )
		{
			powerL /= g_FineTuneFactor;
			powerR /= g_FineTuneFactor;
		}



		// CONTROLLER_2 has the same masking implementation as CONTROLLER_1.
		// For a detailed explanation of the mechanism, see those comments.

		// CONTROLLER_2 is only tested for button X (currently).

		//if ( joystick.joy2_Buttons != false )
		//if ( (g_ControllerMaskB & joystick.joy2_Buttons) != false )
		{

			// If X is pressed, the MOO shall be released!
			if ( Joystick_Button(BUTTON_X, CONTROLLER_2)==true )
			{
				//StartTask(sub_MOO);
				PlaySoundFile("moo.rso");
			}
			if ( Joystick_Button(BUTTON_Y, CONTROLLER_2)==true )
			{
				//StopTask(sub_MOO);
				sub_CowsWithGuns();
			}

		}



		switch (isLiftState)
		{
			case LIFT_BOTTOM:
				sub_LiftToBottom();
				break;
			case LIFT_MIDDLE:
				sub_LiftToMiddle();
				break;
			case LIFT_TOP:
				sub_LiftToTop();
				break;
		}



		// Flush the controller input buffer periodically (every 1/4 sec?)



		Motor_SetPower(motor_L, powerL);
		Motor_SetPower(motor_R, powerR);
		Motor_SetPower(motor_lift, powerLift);
		Motor_SetPower(motor_popcorn, powerPopcorn);



	}
}
task main()
{
	waitForStart();
	while(true)
	{
		getJoystickSettings(joystick);
		if(abs(joystick.joy1_y1) < 75 && abs(joystick.joy1_y2) > 75)
		{
			if(joystick.joy1_y1 < 75)
			{
				straightNoWait(100);
			}
			else if(joystick.joy1_y1 > -75)
			{
				straightNoWait(-100);
			}
			else
			{
				straightNoWait(joystick.joy1_y1);
			}
		}
		else if(joystick.joy1_y2 < 75)
		{
			if(joystick.joy1_y2 > 75)
			{
				turnNoWait(100, 1);
			}
			else if(joystick.joy1_y2 > -75)
			{
					turnNoWait(-100, 1);
			}
			else if(joystick.joy1_y2 == 0)
			{
				motor[rf] = 0;
				motor[rr] = 0;
			}
			else
			{
				turnNoWait(joystick.joy1_y2, 1);
			}
		}
		else if(joystick.joy1_y1 < 75)
		{
			if(joystick.joy1_y1 > 100)
			{
				turnNoWait(100, 0);
			}
			else if(joystick.joy1_y1 < -100)
			{
					turnNoWait(-100, 0);
			}
			else if(joystick.joy1_y1 == 0)
			{
				motor[lf] = 0;
				motor[lr] = 0;
			}
			else
			{
				turnNoWait(joystick.joy1_y1, 0);
			}
		}
		else
		{
			motor[lf] = 0;
			motor[lr] = 0;
			motor[rf] = 0;
			motor[rr] = 0;
		}
		if(joystick.joy2_y1 < 75)
		{
				if(joystick.joy2_y1 > 100)
			{
				armNoWait(100);
			}
			else if(joystick.joy1_y1 < -100)
			{
					armNoWait(-100);
			}
			else
			{
				armNoWait(joystick.joy2_y1);
			}
		}
		else
		{
			motor[E] = 0;
		}
		if(joy1Btn(7) == 1)
		{
			hand();
		}
		else
		{
			stopHand();
		}
		if(joy1Btn(8) == 1)
		{
			handBack();
		}
		else
		{
			stopHand();
		}

	}
}
task main()
{

	initializeRobot();

	waitForStart(); // Wait for the beginning of autonomous phase.
	StartTask( Zelda_Theme );
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;
// In Inches
float COUNTS_PER_INCH = 90.55;

nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[Block_Chuck] = 0;
servo[IRS_1] = 160;
 wait10Msec(400);
ClearTimer(T1);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 )
{
	if ((SensorValue[SONAR_1] < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
	{
		motor[L_Motor] = 25;
		motor[R_Motor] = 50;
  }
  else if ((SensorValue[SONAR_1] > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
  {
  	motor[L_Motor] = 50;
  	motor[R_Motor] = 25;

  }
  else
  {
  	motor[L_Motor] = 50;
		motor[R_Motor] = 50;
	}

}//End of while.
	motor[L_Motor] = 0;
	motor[R_Motor] = 0;
	wait10Msec(55);

	servo[Block_Chuck] = 255;
	wait10Msec(55);

	servo[Block_Chuck] = 0;
	wait10Msec(55);

	ClearTimer(T1);

while( SensorValue[SONAR_1] < 60)
{
	if ((SensorValue[SONAR_1] < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
	{
		motor[L_Motor] = 25;
		motor[R_Motor] = 50;
  }
  else if ((SensorValue[SONAR_1] > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
  {
  	motor[L_Motor] = 50;


  	motor[R_Motor] = 25;

  }
  else
  {
  	motor[L_Motor] = 50;
		motor[R_Motor] = 50;

	}


}//End of while
	  motor[L_Motor] = 0;
		motor[R_Motor] = 0;
		wait10Msec(55);

		Turn(65);
		wait10Msec(55);

		Move(25,100);
		wait10Msec(55);

		Turn(75);
		wait10Msec(55);

		Move(30 ,100);
		wait10Msec(55);

		servo[Block_Chuck] = 250;
		wait10Msec(55);

		servo[Spring_Release] = 250;
		wait10Msec(55);

		servo[Spring_Release] = 0;
		wait10Msec(55);

		servo[Block_Chuck] = 0;
		wait10Msec(55);
}
Example #16
0
task main ()
{

	waitForStart();   // wait for start of tele-op phase
	StartTask (update_gyro);
	nMotorEncoder[leftmotor] = 0;
	nMotorEncoder[rightmotor] = 0;
	servo[Claw]=110;
	start_motor (25, BACKWARD);
	wait10Msec(200);
	start_motor (25, FORWARD);
	wait10Msec(60);
	motor[leftmotor]=0;
	motor[rightmotor]=0;
	wait10Msec(100);
	while (nMotorEncoder[slideL]<18000)
	{
		motor[slideL]=slide_speed;
		motor[slideR]=slide_speed;
	}
	motor[slideL]=0;
	motor[slideR]=0;

	//start_motor (40, TURNRIGHT); //not reading gyro while running start_motor (0.4 seconds).

	while (happy_angle>-90) //ramp up to 40, skip to 100 and run until 40 degrees. TURNED TOO FAR
	{
		nxtDisplayTextLine (3,"%f",happy_angle);
		motor[leftmotor]=-70;
		motor[rightmotor]=70;
	}
	motor[leftmotor]=0;
	motor[rightmotor]=0;
	wait10Msec(100);
	servo[Claw]=50;
	motor[leftmotor]=60;
	motor[rightmotor]=60;
	wait10Msec(80);
	motor[leftmotor]=0;
	motor[rightmotor]=0;
	wait10Msec(100);
	while (happy_angle>-95) //ramp up to 40, skip to 100 and run until 40 degrees.
	{
		nxtDisplayTextLine (3,"%f",happy_angle);
		motor[leftmotor]=-50;
		motor[rightmotor]=50;
	}
	motor[leftmotor]=0;
	motor[rightmotor]=0;
	motor[leftmotor]=100;
	motor[rightmotor]=100;
	wait10Msec(260);
	motor[leftmotor]=0;
	motor[rightmotor]=0;
	wait10Msec(300);
	while (nMotorEncoder[slideL]>0)
	{
		motor[slideL]=-slide_speed;
		motor[slideR]=-slide_speed;
	}
	motor[slideL]=0;
	motor[slideR]=0;
}
Example #17
0
void InGame::loadGame(std::unique_ptr<StartInfo>& startInfo)
{
	//graphics in loading screen
	sf::RenderWindow& window = config.window;
	tgui::Gui gui;
	gui.setWindow(window);

	gui.setFont(tgui::Font(config.fontMan.get("Carlito-Bold.ttf")));

	tgui::Picture::Ptr background = std::make_shared<tgui::Picture>();
	background->setTexture(config.texMan.get("Book.png"));
	background->setSize(gui.getSize());
	gui.add(background);

	tgui::ProgressBar::Ptr progressBar = std::make_shared<tgui::ProgressBar>();
	gui.add(progressBar);
	progressBar->setPosition(50, 700);
	progressBar->setSize(930, 20);
	progressBar->setMinimum(0);
	progressBar->setMaximum(100);
	progressBar->setText("loading...0%");
	unsigned int percent = 0;

	tgui::Panel::Ptr panel = std::make_shared<tgui::Panel>();
	gui.add(panel);
	panel->setSize(820, 200);
	panel->setPosition(100, 450);
	panel->setBackgroundColor(tgui::Color(192, 192, 192, 150));

	tgui::Label::Ptr tips = std::make_shared<tgui::Label>();
	panel->add(tips);
	tips->setPosition(20, 20);
	tips->setTextSize(24);
	tips->setText("Tips : You can find Burny Sanders in the deep of the desert ruin.");
	
	//*************************************************************************
	//the render loop
	bool loading_complete = false;	//leave the loop when loading completed
	Connection* temp_networkPtr = new Connection;
	if (!temp_networkPtr)
	{
		throw "Failed to create network module!";
	}

	while (window.isOpen() && !loading_complete)
	{
		sf::Event event;
		while (window.pollEvent(event))
		{
			if (event.type == sf::Event::Closed)
				window.close();

			gui.handleEvent(event);
		}

		config.cursor.update();

		//if still loading, update percent
		if (percent < 99)
		{
			if (clock.getElapsedTime() > sf::seconds(0.05))
			{
				percent++;
				std::stringstream ss;
				ss << percent;
				progressBar->setText(sf::String("loading...") + sf::String(ss.str()) + sf::String("%"));
				progressBar->setValue(percent);
				clock.restart();
			}
		}
		else	//else, client: send "ready" signal to server for every 5s; server: start anyway
		{
			progressBar->setText(sf::String("waiting for server..."));
			loading_complete = waitForStart(startInfo, temp_networkPtr);
		}

		window.clear();
		gui.draw();
		window.draw(config.cursor);
		window.display();
	}
	delete temp_networkPtr;
	//*************************************************************************
	//if it is server, start server system...
	if (startInfo->type == StartInfo::TYPE::Server)
	{
		systemPtr = new Gameplay::ServerSystem(config, startInfo);
	}
	else //else it is client, start client system
	{
		systemPtr = new Gameplay::ClientSystem(config, startInfo);
	}

	//create network and interface which is pointing to the game system
	networkPtr = new Gameplay::GameNetwork(systemPtr, startInfo);

	systemPtr->setNetworkPtr(networkPtr);

	interfacePtr = new Gameplay::GameInterface(systemPtr);

	systemPtr->setInterfacePtr(interfacePtr);

	if (startInfo->type == StartInfo::TYPE::Server)
	{
		//send ready signal to every player
		//...
	}
	else //if it is a client
	{
		//wait for server's signal
	}
}
Example #18
0
task main() {
	waitForStart();
  /*int raw = 0;
  int nrm = 0;
  bool active = true;

  // Turn the light on
  LSsetActive(LEGOLS);

  nNxtButtonTask  = -2;

  nxtDisplayCenteredTextLine(0, "Lego");
  nxtDisplayCenteredBigTextLine(1, "LIGHT");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);

  nxtDisplayClearTextLine(7);
  nxtDisplayTextLine(5, "Press [enter]");
  nxtDisplayTextLine(6, "to toggle light");
  wait1Msec(2000);*/

  //while (true) {
    // The enter button has been pressed, switch
    // to the other mode
    /*if (nNxtButtonPressed == kEnterButton) {
      active = !active;
      if (!active)
        LSsetInactive(LEGOLS);
      else
        LSsetActive(LEGOLS);

      // wait 500ms to debounce the switch
      wait1Msec(500);
    }

    nxtDisplayClearTextLine(5);
    nxtDisplayClearTextLine(6);
    raw = LSvalRaw(LEGOLS);
    nrm = LSvalNorm(LEGOLS);
    nxtDisplayTextLine(5, "Raw:  %4d", raw);
    nxtDisplayTextLine(6, "Norm: %4d", nrm);
    wait1Msec(50);
  }*/

	int ser=0,fs1=0;
	servo[servo1]=ser;
  servo[servo2]=215-ser;
  bool rightOfLine = true,touch=false;
  LSsetActive(LEGOLS);
  touch = TSreadState(LEGOTS);
  lsVal = LSvalRaw(LEGOLS);
	nMotorEncoder[arms] = 0; //Initiate Encoder Pos

  wait1Msec(2500);


  while(lsVal<360){
  	lsVal = LSvalRaw(LEGOLS);
  	forward(15);
  }
  stop();
  //reverse(50);
  wait1Msec(500);
  stop();
  wait1Msec(1000);
  while(nMotorEncoder[arms]<30){
		raiseArm(50);
		nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
	}stop();
	wait1Msec(1000);
  while(lsVal<210){
  	nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
  	lsVal = LSvalRaw(LEGOLS);
  	left(50);
  }
  stop();
  wait1Msec(1000);
 	fs1 = HTFreadSensor(HTFS1);
 	nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
	ClearTimer(T1);
	while(time1[T1]<2500){
		nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
	  fs1 = HTFreadSensor(HTFS1);
  	lsVal = LSvalRaw(LEGOLS);
  	touch = TSreadState(LEGOTS);
    nxtDisplayTextLine(5, "Raw:  %4d", lsVal);
		if(lsVal>210){
			while(time1[T1]<2500){
				forward(20);
  			lsVal = LSvalRaw(LEGOLS);
   			nxtDisplayTextLine(5, "Raw:  %4d", lsVal);
			}
			rightOfLine=!rightOfLine;
		}
		else if(lsVal<210){
			if(rightOfLine){
				rotateLeft(50);
			}else{
				rotateRight(50);
			}
		}
	}
	stop();

	ser=100;
	servo[servo1]=ser;
  servo[servo2]=215-ser;

  while(nMotorEncoder[arms]>5){
		lowerArm(50);
		nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
	}stop();
	reverse(50);
	wait1Msec(2500);
}
task main(){
  //Code: drive(auto_command,time);
  //replace auto_command and time with values necessary (if you don't replace them, code won't run)
  //auto_command values are: "rpoint", "lpoint", "up", "down", "rswing", "lswing", "rswingback", "lswingback"
  //time values is the time, in milliseconds, you want the robot to be doing the action specified in auto_command

  //Info:
  //Motor at 75% power travels at 2 feet per second = 60.96 cm
  //Lpoint turn takes 750 ms to turn 90 degrees

  //Use the sonar sensor by: SensorValue[sonarSensor].  It can get a distance from 0cm to 80cm
  //drive("up", 10000);      move forward 10000 milliseconds (10sec)
  //drive("lpoint", 1000);   left point turn for 1000 milliseconds (1sec)
  //drive("up", 10000);      move forward again for 10000 milliseconds (10sec)

  waitForStart();

  long distance = 0;

  /*
  int count = 0;

  string BatteryLevel = externalBatteryAvg;
  string selection = "";


  nxtDisplayCenteredBigTextLine (3, BatteryLevel);
  wait1Msec(3000);
  */

  //0) Move off of home zone
  distance = 1650;
  drive("up", distance);

  //1) Measure offset from walls
  //Convert offset distance into milliseconds

  // Turn left 90 degrees and measure
  distance = 870;
  drive("rpoint", distance);
  //offset_right = (SensorValue[sonarSensor]/60.96) * 1000;

  //Remeasure
  //offset_back = ((SensorValue[sonarSensor]/60.96) * 1000) - 1958.3333;


  //2) Move forward ((Four Feet Seven Inches=139.7 cm)-offset_back)

  distance = 2000;
  drive("up", distance);

  //3) Point turn 90 degrees

  distance = 420;
  drive("rpoint", distance);

  distance = 2000;
  drive("up", distance);
/*
  //4) Move forward ((Four Feet Eight Inches=142.24 cm)-(the robot's length)-offset_right)
/////
  distance = 2333.333333 - robot_length - offset_right;
  drive("up", distance);

  //5) Point turn -33.7 degrees

  distance = 280.833333;
  drive("lpoint", distance);

  //6) Move forward ((Seven Feet 2.5 Inches=219.71 cm)-(the robot's length))

  distance = 3604.1666666 - robot_length;
  drive("up", distance);

  //7) Pause 1 second

  wait1Msec(1000);

  //8) Move backward (Three Inches=7.62 cm)

  distance = 125;
  drive("down", distance);

  //9) Point turn 33.7 degrees

  distance = 280.833333;
  drive("rpoint", distance);

  //10) Measure offset from RED base wall (should be from 5' - 7' = 152.4-213.36 cm)
  offset_right = (SensorValue[sonarSensor]/60.96) * 1000;

  //11) Move backward until (offset=(Two Feet Six Inches = 76.2 cm))
  //OR Move backward (Two Feet Four Inches = 71.12 cm)
  //Robot will move into RED Low Zone

  //while offset_right > 857 {
  //  drive("down", 500);
  //  offset_right = SensorValue[sonarSensor]/60.96;
  //}
  //OR
  distance = 857;
  drive("down", distance);

  //12) Point Turn 90 degrees

  distance = 750;
  drive("rpoint", distance);

  //13) Measure offset
  offset_back = (SensorValue[sonarSensor]/60.96) * 1000;

  //14) Move until (offset=(Ten Feet Six Inches = 320.04 cm)-(half the robot's length))
  //OR Move forward (Nine Feet Eight Inches = 294.64 cm)

  //while offset_back > 5250 {
  //  drive("up", 500);
  //  offset_right = SensorValue[sonarSensor]/60.96;
  //}
  //OR
  distance = 4833.33333;
  drive("up", distance);

  //15) Point Turn 90 Degrees

  distance = 750;
  drive("rpoint", distance);

  //16) Move forward (Five Feet Six Inches = 167.64 cm)

  distance = 2750;
  drive("up", distance);*/
}
Example #20
0
task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.
//////////////////////////////////BEGIN AUTONOMOUS CODE/////////////////////////////////////////////////////////
int dir;
int strength1, strength2, strength3, strength4, strength5;

// the default DSP mode is 1200 Hz.
   tHTIRS2DSPMode mode = DSP_1200;

    // You can switch between the two different DSP modes by pressing the
    // orange enter button

    while (true)
    {
       // set the DSP to the new mode
       if (HTIRS2setDSPMode(IRSeeker, mode))
break; // Sensor initialized

       PlaySound(soundShortBlip);
       nxtDisplayCenteredTextLine(6, "Connect Sensor");
       nxtDisplayCenteredTextLine(7, "to Port S2");
       wait1Msec(100);
    }

    eraseDisplay();

    while (true)
    {
       // read the current modulated signal direction
       dir = HTIRS2readACDir(IRSeeker);
       if (dir < 0)
         break; // I2C read error occurred

       if (!HTIRS2readAllACStrength(IRSeeker, strength1, strength2, strength3, strength4, strength5 ))
         break; // I2C read error occurred

       string tmpStr;

StringFormat(tmpStr, "Direction: %d", dir);
nxtDisplayTextLine(3, tmpStr);
StringFormat(tmpStr, "Strength: %d", strength3);
nxtDisplayTextLine(4, tmpStr);

if (dir < 5) {
// turn right
motor[driveRight] = 100;
motor[driveLeft] = -100;
} else if (dir > 5) {
// turn left
motor[driveRight] = -100;
motor[driveLeft] = 100;
} else if (strength3 < 110) {
motor[driveRight] = 100;
motor[driveLeft] = 100;
} else {
motor[driveRight] = 0;
motor[driveLeft] = 0;
}
    }


  while (true)
  {}
}
Example #21
0
task main()
{
	int elevatorHeightTicks;

	// Set the following variables to false to hide the standard NXT LCD information
	bDisplayDiagnostics = false;
	bNxtLCDStatusDisplay = false;

	////////// INITIALIZATIONS //////////
	initializeRobot();	// Execute robot initialization routine

	///////////////  Variables to be used later  /////////////////
	//int maxArmHeightTicks = inchesToTicks(maxArmHeight);
	//int minRingPickupHeightTicks = inchesToTicks(minRingPickupHeight);
	//int WAMservoStep = 3;	//Amount to inc1rement the WAM servo position
	//int WAMservoStep12 = 17;	// Move a servo 15 degrees
	//float maxArmHeight = 45.25;	// Maximum Safe Arm Height used during manual control of the arm
	//float minRingPickupHeight = 8.0;

	// User selected Autonomous information
	{
		selectStartLocation();	// Get the starting location of the robot
		selectAutoActions(); 		// Get Autononmous actions
		switch(AutoActions)
		{
		case 0:	// No Autonomous
			break;
		case 1:	// IR Beacon
			selectRow();  // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 2:	// Left Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 3:	// Center Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 4:	// Right Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 5:	// Play Defense
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 6:	// Left Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		default:	// Not a valid selection
			eraseDisplay();
			nxtDisplayCenteredTextLine(1,"DITU SAYS");
			nxtDisplayCenteredTextLine(2,"INPUT ERROR");
			nxtDisplayCenteredTextLine(4,"TRY AGAIN");
			wait1Msec(6000);
			break;
		}
	}
	eraseDisplay();
	// Set the following variables to false to hide the standard NXT LCD information
	// Show the default FTC Display Information
	bDisplayDiagnostics = true;
	bNxtLCDStatusDisplay = true;
	//bDisplayDiagnostics = false;
	//bNxtLCDStatusDisplay = false;

	// Determine the autonomous start delay based on delayAutoStart
	switch(delayAutoStart)
	{
	case 0:	// delay start = 0 seconds
		delayAutoStartValue = 0;
		break;
	case 1:	// delay start = 1 second
		delayAutoStartValue = 1000;
		break;
	case 2:	// delay start = 5 seconds
		delayAutoStartValue = 5000;
		break;
	case 3:	// delay start = 10 seconds
		delayAutoStartValue = 10000;
		break;
	case 5:	// delay start = 15 seconds
		delayAutoStartValue = 15000;
		break;
	default:	// delay start = 0 seconds
		delayAutoStartValue = 0;
		break;
	}

	// Process robot starting location selection
	switch(robotStartLocation)
	{
	case 0:	// Left Wall
		leftWall = true;
		break;
	case 1:	// Corner
		corner = true;
		break;
	case 2:	// Right Wall
		rightWall = true;
		break;
	default:	// Not a valid starting location
		break;
	}

	// Process robot starting location selection
	switch(Row)
	{
	case 0:	// Bottom Row
		pegHeightCmd = 1;
		break;
	case 1:	// Middle Row
		pegHeightCmd = 2;
		break;
	case 2:	// Top Row
		pegHeightCmd = 3;
		break;
	default:	// Not a valid starting location
		break;
	}

	waitForStart();	// Wait for the signal to start from the Field Control System

	//reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually
	ProcessProto();

	switch(AutoActions)
	{
	case 0:	// No Autonomous
		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 1 || Score Ring on the column designated by the IR Beacon===============================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 1:	// Score Ring on the column designated by the IR Beacon

		wait1Msec(delayAutoStartValue);

		// INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE
		// move forward, turn 45 degrees, move forward
		move_forward(24, 2000, 90, 90);
		wait10Msec(50);
		//move(1, 0, 1000);

		clearTimer(T1);

		//HTIRS2setDSPMode(InfraredSensor, DSP_1200);
		while (time1[T1] < 500)
		{
		irSensorVal = SensorValue[IR];
		nxtDisplayBigTextLine(2, "IR: %d", irSensorVal);
		}

		//////////////////////////////////////////////////////////////////

		switch(irSensorVal)
		{
		case 1:
		turngyro_left(-90.0, 35);
		wait10Msec(100);
		move_forward(24, 4000, 90, 90);
		wait10Msec(100);
		turngyro_left(83.0, 30);
		wait10Msec(100);
		move_forward(.5, 2000, 90, 90);
		wait1Msec(50);
		break;

		//------------------------------------------------------------------

		case 2:
		wait10Msec(100);
		move_backwards(2, 1000, 90, 90);
		turngyro_left(-45.0, 50);
		wait10Msec(50);
		ClearTimer(T1);
		nMotorEncoder[Right2] = 0;
		nMotorEncoder[Left2] = 0;
		move_forward(56, 4000, 90, 90);
		ClearTimer(T1);
		move_backwards(12.5, 2000, 90, 90);
		wait1Msec(50);
		break;

		//-----------------------------------------------------

		case 3:
		move_forward(43, 2000, 90, 90);
		wait10Msec(100);
		turngyro_left(-90.0, 50);
		wait10Msec(100);
		move_forward(2, 1000, 90, 90);
		wait1Msec(50);
		break;
		}

		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring

		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 2 || Score Ring on Left Column==========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 2:	// Score Ring on Left Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring
		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 3 || Score Ring on Center Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 3:	// Score Ring on Center Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 	10000);	// Raise elevator to the commanded height
		if (tripped) //If the elevator trips the fail safe, break
			break;

		wait1Msec(500);

		move_forward(60-5, 5000, 90, 90);

		/////The alignment from the wall that could work.../////
		//move_forward(24, 4000, 80, 80);
		//turngyro_left(45, 60, 60);
		//move_forward(4, 2000, 80, 80);

		RAM_down();

		// Drive forward until the RAM limit switch is activated or time expires
		ClearTimer(T1);
		while(ramLimit==0 && time1[T1] < 2500)
		{
			// Try to follow the white navigation tape
			if(SensorValue(colorSensorVal) >= 15)
			{
				// The color sensor sees the black platform, command the robot to drift to the right
				motor[Left1] = 35;
				motor[Left2] = 35;
				motor[Right1] = 15;
				motor[Right2] = 15;
			}
			else
			{	// The color sensor sees the white navigation tape, command the robot to drift to the left
				motor[Left1] = 15;
				motor[Left2] = 15;
				motor[Right1] = 35;
				motor[Right2] = 35;
			}
			ProcessProto();
		}
		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = -25;
			motor[Left2] = -25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = 20;
			motor[Left2] = 20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = 25;
			motor[Left2] = 25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		/*ClearTimer(T1);
		while(time1[T1] < 200)
		{
		motor[Left1] = -20;
		motor[Left2] = -20;
		motor[Right1] = -20;
		motor[Right2] = -20;
		motor[RAMright] = 95;
		motor[RAMleft] = 95;
		}*/

		motor[RAMright] = 0;
		motor[RAMleft] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;
		motor[Right1] = 0;
		motor[Right2] = 0;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Raise elevator to the commanded height to score a ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Score Ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 4 || Score Ring on Right Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 4:	// Score Ring on Right Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here
		//asdfjkl;

		wait1Msec(500);

		move_forward(12, 5000, 90, 90);
		turngyro_right(45, 100);
		move_forward(20, 7000, 80, 80);
		//RAM_down();

		motor[Right1] = 0;
		motor[Right2] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;


		// Drive forward until the RAM limit switch is activated or time expires
		ClearTimer(T1);
		while(ramLimit==0 && time1[T1] < 2500)
		{
			// Try to follow the white navigation tape
			if(SensorValue(colorSensorVal) >= 15)
			{
				// The color sensor sees the black platform, command the robot to drift to the right
				motor[Left1] = 35;
				motor[Left2] = 35;
				motor[Right1] = 15;
				motor[Right2] = 15;
			}
			else
			{	// The color sensor sees the white navigation tape, command the robot to drift to the left
				motor[Left1] = 15;
				motor[Left2] = 15;
				motor[Right1] = 35;
				motor[Right2] = 35;
			}
			ProcessProto();
		}
		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = -25;
			motor[Left2] = -25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 20;
			motor[Left2] = 20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 25;
			motor[Left2] = 25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = -20;
			motor[Left2] = -20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		motor[RAMright] = 0;
		motor[RAMleft] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;
		motor[Right1] = 0;
		motor[Right2] = 0;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Raise elevator to the commanded height to score a ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Score Ring
		if (tripped) //If the elevator trips the fail safe, break
			break;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring
		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 5 || Play Defense=======================================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 5:	// Play Defense
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		//wait1Msec(500);
		move_backwards(84.0, 10000, 100, 100);
		break;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 6 || Score Center & Line Up with the Dispsenser=========================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	case 6:	// Score on the center column & line up with the dispenser
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 	10000);	// Raise elevator to the commanded height
		if (tripped) //If the elevator trips the fail safe, break
			break;

		wait1Msec(500);

		move_forward(60-5, 5000, 90, 90);
		RAM_down();

		// Drive forward until the RAM limit switch is activated or time expires
		ClearTimer(T1);
		while(ramLimit==0 && time1[T1] < 2500)
		{
			// Try to follow the white navigation tape
			if(SensorValue(colorSensorVal) >= 15)
			{
				// The color sensor sees the black platform, command the robot to drift to the right
				motor[Left1] = 35;
				motor[Left2] = 35;
				motor[Right1] = 15;
				motor[Right2] = 15;
			}
			else
			{	// The color sensor sees the white navigation tape, command the robot to drift to the left
				motor[Left1] = 15;
				motor[Left2] = 15;
				motor[Right1] = 35;
				motor[Right2] = 35;
			}
			ProcessProto();
		}
		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = -25;
			motor[Left2] = -25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = 20;
			motor[Left2] = 20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = 25;
			motor[Left2] = 25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		motor[RAMright] = 0;
		motor[RAMleft] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;
		motor[Right1] = 0;
		motor[Right2] = 0;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Raise elevator to the commanded height to score a ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Score Ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		move_backwards(40-5, 5000, 90, 90);
		turngyro_right(135, 100);
		move_forward(20-2, 5000, 90, 90);
		turngyro_left(90, 100);
		move_forward(10-2, 5000, 90, 90);
		turngyro_left(90, 100);
		move_backwards(10-2, 5000, 90, 90);
		break;

	default:	// Not a valid autonomous action
		break;
	}
	servo[colorSensorServo] = colorServoDefault;
	wait10Msec(200);
	// All Done, time to stop all tasks
	StopAllTasks();
}	// End Main Bracket
task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    servo[handJoint] = 240;
    wait1Msec(100);

    black_threshold = LSvalNorm(middle_light);
    white_threshold = black_threshold + 5;

    writeDebugStreamLine("black_threshold: %d", black_threshold);
    writeDebugStreamLine("white_threshold: %d", white_threshold);

    //
    // Move to other side of field
    //
    moveStraight(50,500);
    //
    // Find the line
    //
    // Continue forward until the middle sensor detects the white line
    //
    writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));

    while (LSvalNorm(middle_light) < white_threshold) {

        //!--Code for debugging
        middle_nrm = LSvalNorm(middle_light);
        if (middle_nrm != temp_middle_nrm) {
            writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
            temp_middle_nrm = middle_nrm;
        }
        //--!

        setAllMotorVals(20);
    }
    allStop();

    //
    // Turn 90 degrees
    //
    // Move forward a little more and then turn until the middle sensor detects the white line
    //

    bool isLeft = true; // If this boolean is true, the 90 degree turn is toward the left

    moveStraight(30,200);
    //moveStraight(15,1000); // with long arm

    // Make a variable that counts the number of times through the loop. If it goes above a certain
    // threshold, assume the robot is stuck on the edge of the wood and increase the power
    int counter1 = 0;

    while (LSvalNorm(middle_light) < white_threshold) {

        //!--Code for debugging
        middle_nrm = LSvalNorm(middle_light);
        if (left_nrm != temp_left_nrm) {
            writeDebugStreamLine("left light sensor value: %d", middle_nrm);
            temp_left_nrm = left_nrm;
        }
        //--!

        if (isLeft) {
            if (counter1 > 500) {
                setDriveMotorVals(60,-60);
            }
            else {
                setDriveMotorVals(40,-40);
            }
        }
        else {
            if (counter1 > 100) {
                setDriveMotorVals(-20,20);
            }
            else {
                setDriveMotorVals(-15,15); // with long arm
            }
        }
        counter1 += 1;
    }

    //
    // Follow the line until touch sensor is triggered
    //
    //	1. Left on white, middle on white (or black), right on black: turn toward the left
    //	2. Left on black, middle on white (or black), right on white: turn toward the right
    //	3. Left on black, middle on white, right on black: move straight
    //	4. Left on black, middle on black, right on black: lost...
    //

    // Set up touch sensor variables
    int nButtonsMask;
    bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
    // If one has, this changes to true.

    // Make a variable that counts the number of times through the loop. If it goes above a certain
    // threshold, assume the robot is stuck on the edge of the wood and increase the power
    int counter2 = 0;

    //deploy the spear
    deploySpear(true);

    while (isTouch == false) {

        writeDebugStreamLine("-----------------counter2 val: %d", counter2);

        // Read light sensor values
        left_nrm = LSvalNorm(left_light);
        middle_nrm = LSvalNorm(middle_light);
        right_nrm = LSvalNorm(right_light);

        //!--Code for debugging
        //if (left_nrm != temp_left_nrm) {
        writeDebugStreamLine("left light sensor value: %d", left_nrm);
        //temp_left_nrm = left_nrm;
        //}
        //if (middle_nrm != temp_middle_nrm) {
        writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
        //temp_middle_nrm = middle_nrm;
        //}
        //if (right_nrm != temp_right_nrm) {
        writeDebugStreamLine("middle light sensor value: %d", right_nrm);
        //temp_right_nrm = right_nrm;
        //}
        //--!

        if (left_nrm < black_threshold) {
            // left sensor is black

            if (middle_nrm < black_threshold) {
                // middle sensor is black

                if (right_nrm > black_threshold) {
                    // right sensor is white or gray
                    // turn right
                    writeDebugStreamLine("Case 1: turned right");

                    if (counter2 > 600) {
                        setDriveMotorVals(30,0);
                    }
                    else {
                        setDriveMotorVals(20,0);
                    }
                }
                else {
                    // right sensor is black
                    // lost, so just turn right a lot

                    setDriveMotorVals(30,-30);
                }
            }

            else {
                // middle sensor is not black

                if (middle_nrm > white_threshold) {
                    // middle sensor is white
                    // assume right sensor is black
                    // move straight

                    writeDebugStreamLine("Case 2: went straight");

                    if (counter2 > 600) {
                        setAllMotorVals(30);
                    }

                    else {
                        setAllMotorVals(20);
                    }
                }
                else {
                    // middle sensor is gray
                    // assume right sensor is gray
                    // turn right

                    writeDebugStreamLine("Case 3: turned right");

                    if (counter2 > 600) {
                        setDriveMotorVals(30,0);
                    }
                    else {
                        setDriveMotorVals(20,0);
                    }
                }
            }
        }
        else {
            // left sensor is not black

            if (left_nrm > white_threshold) {
                // left sensor is white
                // assume middle sensor is black
                // assume the right sensor is black too
                // turn left

                writeDebugStreamLine("Case 4: turned left");

                if (counter2 > 600) {
                    setDriveMotorVals(0,30);
                }
                else {
                    setDriveMotorVals(0,20);
                }
            }
            else {
                // left sensor is gray
                // middle sensor is gray
                // assume right sensor is black
                // turn left

                writeDebugStreamLine("Case 5: turned left");

                if (counter2 > 600) {
                    setDriveMotorVals(0,30);
                }
                else {
                    setDriveMotorVals(0,20);
                }
            }
        }

        counter2 += 1;

        // Read touch sensor values
        nButtonsMask = SensorValue[S3];

        if (nButtonsMask & 0x01)
            isTouch = true;
        if (nButtonsMask & 0x02)
            isTouch = true;
    }
    allStop();
    wait1Msec(1000);

    //
    // More later....scoring stuff
    //
    //	1. Move backward a little
    //	2. Unfold arm
    //	3. Move forward a little
    //	4. Move arm down to score
    //	5. Back up
    //	6. Reset arm
    //	7. Move toward our rings?
    //


    // back up a little
    moveStraight(-20,800);

    // unfold arm
    fold_arm(false);

    // move forward a little
    moveStraight(20,600);

    // move arm down
    motor[shoulderJoint] = -50;
    wait1Msec(800);
    motor[shoulderJoint] = 0;

    // back up
    moveStraight(-40,500);

    // reset arm
    fold_arm(true);

    //retract spear
    //deploySpear(false);

    while (true) {
        return;
    }
}
task main()
{
	initializeRobot();

	waitForStart(); // Wait for the beginning of autonomous phase.

	//save IR state here
	//determine what position the beacon is at
	int _dir =HTIRS2readACDir(IRseek);
	//To do test values for range
	if (_dir == 1)
	{
		position =1;
	}
	else if(_dir == 2)
	{
		position =2;
	}
	else if(_dir == 3)
	{
		position =3;
	}
	else if(_dir == 4)
	{
		position =4;
	}
	else if(_dir == 5)
	{
		position =5;
	}else if(_dir == 0)
	{
		position =0;
	}

	if (position ==1)
	{
		if ((_dir)!=1)
		{
			motor[Right]=75;
			motor[Left]=75;
		}
		else{
			motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(200);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);

	}
	if (position ==2)
	{
			if ((_dir)!=1)
		{
			motor[Right]=75;
			motor[Left]=75;
		}
				motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(250);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);
	}
	if (position ==3)
	{
			if ((_dir)!=1)
		{
			motor[Right]=75;
			motor[Left]=75;
		}
				motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(300);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);
	}
	if (position ==4)
	{
			if ((_dir)!=1)
		{
			motor[Right]=75;
			motor[Left]=75;
		}
				motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(350);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);
	}
	if (position ==5)
	{

			if ((_dir)!=1)
		{
			motor[Right]=75;
			motor[Left]=75;
		}

				motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(400);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);
	}
	if (position ==0)
	{
		motor[Right]=0;
		motor[Left]=0;
		wait1Msec(500);

		motor[Arm]=75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		servo[Claw]=150;

		motor[Right]=-75;
		motor[Left]=-75;
		wait10Msec(150);

		motor[Right]=75;
		motor[Left]=-75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(200);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(50);

		motor[Right]=-75;
		motor[Left]=75;
		wait10Msec(100);

		motor[Right]=75;
		motor[Left]=75;
		wait10Msec(150);
	}
	}

}
Example #24
0
task main()
{
    short right_y;
    short left_y;
    short left_dock_y;

    debounce = false;

    initializeRobot();

    waitForStart();   // wait for start of tele-op phase

    // StartTask(endGameTimer);

    while (true) {

        getJoystickSettings(joystick);

        if (!debounce) {
 	        if (joy2Btn(Btn1)) {
	            handle_joy2_event(BUTTON_ONE);
	        } else if (joy2Btn(Btn2)) {
	            handle_joy2_event(BUTTON_TWO);
	        } else if (joy2Btn(Btn3)) {
	            handle_joy2_event(BUTTON_THREE);
	        } else if (joy2Btn(Btn4)) {
	            handle_joy2_event(BUTTON_FOUR);
	        } else if (joy2Btn(Btn5)) {
	            handle_joy2_event(LEFT_TRIGGER_UP);
	        } else if (joy2Btn(Btn6)) {
	            handle_joy2_event(RIGHT_TRIGGER_UP);
	        } else if (joy2Btn(Btn7)) {
	            handle_joy2_event(LEFT_TRIGGER_DOWN);
	        } else if (joy2Btn(Btn8)) {
	            handle_joy2_event(RIGHT_TRIGGER_DOWN);
            }
       }

        //if (drive_multiplier) {
            //right_y = joystick.joy1_y2;
            //left_y = joystick.joy1_y1;
        //} else {
            left_dock_y = joystick.joy2_y1;
            right_y = joystick.joy1_y1;
            left_y = joystick.joy1_y2;
        //}

        if (abs(right_y) > 20) {
	    	motor[driveFrontRight] = drive_multiplier * left_y;
	    	motor[driveRearRight] = drive_multiplier * left_y;
		}
		else {
		    motor[driveFrontRight] = 0;
		    motor[driveRearRight] = 0;
		}

        if (abs(left_y) > 20) {
		    motor[driveFrontLeft] = drive_multiplier * right_y;
		    motor[driveRearLeft] = drive_multiplier * right_y;
		}
		else
		{
		    motor[driveFrontLeft] = 0;
		    motor[driveRearLeft] = 0;
		}

        if (abs(left_dock_y) > 20) {
            if (left_dock_y > 0) {
                servo[dockarm] = SERVO_DOCK_ARM_FORWARD;
            } else {
                servo[dockarm] = SERVO_DOCK_ARM_BACKWARD;
            }
        } else {
            servo[dockarm] = SERVO_DOCK_ARM_STOPPED;
        }
    }
}
task main()
{

	waitForStart();
	int beacon = 0; //holds which beacon the robot is at.
	int i = 0; 			//holds the IR sensor value.

	//loops until the sensor finds the beacon or until it gets to the final beacon.
	while(i != 5 && beacon < 4)
	{
		i=0;
		//this section prevents the reading of random IR values and makes sure the value is constant.
		//when the value is the constant we want, the beacon is in front of the sensor.
		for(int a = 0; a <100; a++)
		{
			if(a == 0)
			{
				//gets original ir value
				i  = SensorValue[IR];
			}
			else
			{
				int b = SensorValue[IR];
				if(b != i)
				{
					//if a different value is sensed, then the original value was not constant; thus, we restart the checking
					//to try and gete a constant value.
					i = 0;
					a = 0;
				}
			}
		}
		if(i != 5)
		{
			if(beacon == 0)
			{
				//the distance to get to the first beacon
				ForwardWithModTime(1200,35);
				motor[Right] = 0;
				motor[Left] = 0;
				wait1Msec(1000);
			}
			else if(beacon == 1)
			{
				//the distance to get to the third beacon
				ForwardWithModTime(450,35);
				motor[Right] = 0;
				motor[Left] = 0;
				wait1Msec(1000);
			}
			else if(beacon == 2)
			{
				//the distance to get to the third beacon
				ForwardWithModTime(900,35);
				motor[Right] = 0;
				motor[Left] = 0;
				wait1Msec(1000);
			}
			else if(beacon == 3)
			{
				//the distance to get to the third beacon
				ForwardWithModTime(470,35);
				motor[Right] = 0;
				motor[Left] = 0;
				wait1Msec(1000);
			}
			beacon++;
		}
	}

	//code that runs after  we stop in front of a beacon.
		servo[AutoDispenser] = 255;	//dispenses the cube
		wait1Msec(900);
		servo[AutoDispenser] = 0;
		wait1Msec(1000);

	//pick where to  go based on beacon #.
	//if the robot is at rings 1 or 4, it turns, goes back, then turns to face the ramp.
	//if the robots is at rings 2 or 3, it goes backwards, turns, goes backwards again, and then turns to face the ramp.
	if(beacon == 1) //beacon was on first ring
	{
		BackwardWithModTime(800,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = -100;
		motor[Left] = 100;
		wait1Msec(700);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		BackwardWithModTime(1600,50);
		motor[Right] = 100;
		motor[Left] = -100;
		wait1Msec(900);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		ForwardWithModTime(1400,100);
	}
	else if (beacon == 2) //beacon was on second ring
	{
		BackwardWithModTime(1200,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = -100;
		motor[Left] = 100;
		wait1Msec(700);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		BackwardWithModTime(1600,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = 100;
		motor[Left] = -100;
		wait1Msec(900);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		ForwardWithModTime(1400,100);
	}
	else if (beacon == 3) //beacon was on third ring
	{
		ForwardWithModTime(1000,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = 100;
		motor[Left] = -100;
		wait1Msec(900);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		ForwardWithModTime(1600,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = -100;
		motor[Left] = 100;
		wait1Msec(900);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		BackwardWithModTime(1400,100);
	}
	else if (beacon == 4) //beacon was on fourth ring
	{
		ForwardWithModTime(800,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = 100;
		motor[Left] = -100;
		wait1Msec(700);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		ForwardWithModTime(1600,50);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		motor[Right] = -100;
		motor[Left] = 100;
		wait1Msec(800);
		motor[Right] = 0;
		motor[Left] = 0;
		wait1Msec(500);

		BackwardWithModTime(1500,100);
	}
}
task main()
{
	waitForStart();
	initializeRobot();

	while(true)
	{
		getJoystickSettings(joystick);
		int jLeft = (int)joystick.joy1_y1;
		int jRight = -(int)joystick.joy1_y2;


		//Driver Controls//////////////////////////////////////////////////////////////////////////

		//////Drivetrain///////////////////////////////////////////////////////////////////////
		//Speed Control//
		if(joy1Btn(5))
		{
			speed = true;
		}
		//------------------------------
		if(joy1Btn(7))
		{
			speed = false;
		}
		//-------------------------------
		if (speed)
		{
			if (abs(jLeft) < 10)            ///< core out the noise for near zero settings
				motor[LeftMotor] = 0;         ///< sets the left motor to 0% power
			else
				motor[LeftMotor] = jLeft;     ///< set motors to joystick settings
			if (abs(jRight) < 10)           ///< core out the noise for near zero settings
				motor[RightMotor] = 0;        ///< sets the right motor to 0% power
			else
				motor[RightMotor] = jRight;   ///< sets motors to joystick settings
		}
		//--------------------------------
		else
		{
			if (abs(jLeft) < 10)              ///< core out the noise for near zero settings
				motor[LeftMotor] = 0;           ///< sets the left motor to 0% power
			else
				motor[LeftMotor] = (jLeft/3);   ///< set motors to joystick settings
			if (abs(jRight) < 10)             ///< core out the noise for near zero settings
				motor[RightMotor] = 0;          ///< sets the right motor to 0% power
			else
				motor[RightMotor] = (jRight/3); ///< sets motors to joystick settings
		}

		//CURRENTLY NULL//
		/*

		//////BAM////////////////////////////////////////////////////////////////////////////////
		//up
		if(joy1Btn(8))
		{
			servoTarget[bamLeft] = 255;
			servoTarget[bamRight] = 0;
		}
		//down
		else if(joy1Btn(6))
		{
			servoTarget[bamLeft] = 0;
			servoTarget[bamRight] = 255;
		}
		//Idle
		else
		{
			servoTarget[bamLeft] = 127;
			servoTarget[bamRight] = 127;
		}

		*/

		//Manipulator Controls/////////////////////////////////////////////////////////////////////

		//////lifter///////////////////////////////////////////////////////////////////////////////

		int touchVal;
		touchVal = SensorValue(touch);

		int lifterEncoderVal;
		lifterEncoderVal = nMotorEncoder[lifter];

		nxtDisplayCenteredBigTextLine(2, "%d", touchVal);
		nxtDisplayCenteredBigTextLine(5, "%d", lifterEncoderVal);

		//up
		if(joy2Btn(7) && (touchVal == 0))
		{
			motor[lifter] = -50;
		}
		//down
		else if(joy2Btn(5))
		{
			motor[lifter] = 50;
		}
		//Idle
		else
		{
			motor[lifter] = 0;
		}


		//OVERRIDE
		//down
		if(joy2Btn(4))
		{
			motor[lifter] = 50;
		}

		/*
		float encoderVal;
		nMotorEncoder[lifter] = encoderVal;
		nxtDisplayBigTextLine(2, "encoder val: %lf", encoderVal);
		*/

		/*

		//////Mechanical Stop/////////////////////////////////////////////////////////////////////////
		//Down////
		if(joy2Btn(2))
		{
			nMotorEncoderTarget[stopLeft] = 90;
			nMotorEncoderTarget[stopRight] = 90;
			ClearTimer(T1);
			while ((nMotorEncoder[stopLeft] < 90) && (time1[T1] < 500))
			{
				motor[stopLeft] = -30;
				motor[stopRight] = -30;
			}
		}

		//Up////
		if(joy2Btn(4))
		{
			nMotorEncoderTarget[stopLeft] = 0;
			nMotorEncoderTarget[stopRight] = 0;
			ClearTimer(T1);
			while ((nMotorEncoder[stopLeft] > 0) && (time1[T1] < 500))
			{
				motor[stopLeft] = 30;
				motor[stopRight] = 30;
			}
			motor[stopLeft] = 0;
			motor[stopRight] = 0;
		}

		*/

		//Ramp/////////////////////////////////////////////////////////////////////////
		if(joy2Btn(10))
		{
			ClearTimer(T1);
			while(time1[T1] < 1000)
			{
				servoTarget[ramp] = 255;
			}
		}
	}
}
task main()
{
		//use NXT buttons to select autonomous route and time delay

		initializeRobot();

  	waitForStart(); // Wait for the beginning of autonomous phase.
		UnlockRightHook();
		UnlockLeftHook();
		int LeftIRVal = HTIRS2readACDir(LeftIR); //Initialize variable storing left IR sensor value



		int timeElapsed = 0; //define variable timeElapsed which will store time values
		wait10Msec(500);
		time1[T1] = 0; //reset the timer
		timeElapsed = time1[T1] //set time elapsed to the timer value
		while(LeftIRVal != 5) //move forward until robot is perpendicular to IR beacon
		{
			motor[LeftDrive] = 100;
			motor[RightDrive] = 100;
			wait10Msec(1);

			LeftIRVal = HTIRS2readACDir(LeftIR); //refresh the current IR value
		}
		timeElapsed = time1[T1] - timeElapsed;
			motor[LeftDrive] = 0; //Brake Motors
			motor[RightDrive] = 0;

		nxtDisplayCenteredTextLine(3, "%d", nMotorEncoder[RightDrive]);//display current encoder values for drive train motors on screen

		////////////////////////////
		//deploy autonomous block://
		////////////////////////////

		//Move(70, 70, 1000, 1000);
		if(timeElapsed < 1500) // if time elapsed to find beacon is less than 2000 ms, then probably we mov a little more
		{
				motor[LeftDrive] = 50;
				motor[RightDrive] = 50;
				wait1Msec(350);
				motor[LeftDrive] = 0;
				motor[RightDrive] = 0;
		}
		else //Else just deploy right there
		{
				motor[LeftDrive] = 0;
				motor[RightDrive] = 0;
		}
		ServoRotate (AutonomousBlock,30); //Rotating Servo to put block in basket
		ServoRotate (AutonomousBlock,31);
		ServoRotate (AutonomousBlock,130);//Rotate Servo inside robot

		wait10Msec(10);

		/////////////////
		//go onto ramp://
		/////////////////

		while(1)
		{
			if(nMotorEncoder[RightDrive] < 500) // Go forward until a certain total distance is reached
			{
				motor[RightDrive] = 70;
			  motor[LeftDrive] = 70;
			}
			else //once that distance is reached, stop the robot
			{
				motor[RightDrive] = 0;
	 			motor[LeftDrive] = 0;
	 		}
		}

		Move(-60, 60, 2150, 2150);  //turn robot so that backside faces ramp
		Move(60, 60, 3100, 3100);   //Move forward toward ramp
		Move(-60, 60, 2150, 2150);  //Turn perpendicular to ramp
		Move(100, 100, 4000, 4000); //Drive on to ramp at full force

		motor[RightDrive] = 0; //stop robot and park on ramp
 		motor[LeftDrive] = 0;

		while(1){wait10Msec(10);} //wait until end of autonomous time period
}
//=========================================
// Main Program
//=========================================
task main()
{
	disableDiagnosticsDisplay();
	alive();
	initializeRobot();

	StartTask(sensors);
	StartTask(selector);
	StartTask(display);

	motor[lightMotor] = 50;
	wait1Msec(500);
	motor[lightMotor] = 0;

	waitForStart();

	constHeading = 0;
	relHeading = 0;

	if(calibrate != 2)
	{
		gyroCalTime = 3;
		calibrate = 1;
		while(calibrate != 2)
		{
			EndTimeSlice();
		}
	}
	constHeading = 0;
	relHeading = 0;

	wait1Msec(time_selector*1000);
	PlaySound(soundBeepBeep);

	switch(MissionNumber)
	{
		//================================================
		// start on the right side of the blue dispencer delivers to IR, then stops
		//================================================
	case 1:

		GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(1000);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,18,true,true,false,false);

		if(currDir >= 4 && currDir <= 6) column = 2;
		if(currDir < 4) column = 1;
		if(currDir > 6) column = 3;

		switch(column)
		{
		case 1:
			GyroTimeS_moveV2(300,30,false,false,true,REL);
			GyroTimeS_moveV2(8000,15,true,true,true,REL);
			motor[RB_motor] = 0;
			motor[RF_motor] = 0;
			motor[LF_motor] = 0;
			motor[LB_motor] = 0;

			GyroTime45_V2(340,-30,true,true,REL,true);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(800,-30,true,false,REL);

			motor[LB_motor] = 50;
			motor[LF_motor] = 50;
			wait1Msec(700);
			motor[RF_motor] = 50;
			wait1Msec(100);
			motor[LB_motor] = 0;
			motor[LF_motor] = 0;
			motor[RF_motor] = 0;
			wait1Msec(1000);
			servo[shoulder] = 0;
			wait1Msec(2000);
			servo[wrist] = 130;
			wait1Msec(800);
			break;
		case 2:
			GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
			wait1Msec(1500);
			servo[shoulder] = 0;
			wait1Msec(2000);
			servo[wrist] = 130;
			wait1Msec(3000);
			GyroTime_moveV2(300,30,true,false,false);
			wait1Msec(3000);
			servo[shoulder] = 255;
			wait1Msec(3000);
			servo[wrist] = 130;
			wait1Msec(800);
		case 3:
			GyroTimeS_moveV2(300,-15,false,true,true,false);
			GyroTimeS_moveV2(8000,-15,true,true,true,false);
			GyroTimeS_moveV2(260,-15,false,true,false,false);
			wait1Msec(1500);
			servo[shoulder] = 0;
			wait1Msec(2000);
			servo[wrist] = 130;
			wait1Msec(3000);
			GyroTime_moveV2(300,30,true,false,false);
			wait1Msec(3000);
			servo[shoulder] = 255;
			wait1Msec(3000);
			servo[wrist] = 130;
			wait1Msec(800);
			break;
		}
		break;
		//================================================
		// start on the right side of the blue dispencer and delivers to the center column, then stops
		//================================================
	case 2:
		GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(1000);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,18,true,true,false,false);

		GyroTimeS_moveV2(220,-25,false,true,false,false);//was 15
		wait1Msec(1500);
		servo[shoulder] = 0;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(3000);
		GyroTime_moveV2(300,30,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = 255;
		wait1Msec(3000);
		break;
		//================================================
		// start on the right side of the blue dispencer and delvivers to B and returns to the blue dispencer
		//================================================
	case 3:
		GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(1000);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,18,true,true,false,false);

		GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
		wait1Msec(1500);
		servo[shoulder] = 0;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(1500);
		GyroTime_moveV2(300,30,true,false,false);
		wait1Msec(1000);
		servo[shoulder] = 255;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(800);

		Gyro_TurnV2(42,15,REL);
		GyroTime_moveV2(1600,45,true,false,false);
		break;
		//================================================
		// start on the right side of the blue dispencer and delvivers to B and returns to the red dispencer
		//================================================
	case 4:
		GyroSonar_moveV2(0, SIDE_BACK, 125, 110, -60,true, true, CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(1000);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,18,true,true,false,false);

		GyroTimeS_moveV2(260,-25,false,true,false,false);//was 15
		wait1Msec(1500);
		servo[shoulder] = 0;
		wait1Msec(2000);
		servo[wrist] = 130;
		servo[right_servo] = 90;
		servo[left_servo] = 90;
		wait1Msec(1500);
		GyroTime_moveV2(300,30,true,false,false);
		wait1Msec(1000);
		servo[shoulder] = 255;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(800);

		Gyro_TurnV2(42,-15,REL);
		GyroTime_moveV2(1200,50,true,false,REL);
		GyroTime45_V2(490,50,false, true, REL, true);
		GyroTime_moveV2(400,50,true,false,REL);
		break;
		//================================================
		// start on the right side of the blue dispencer and delivers to the left column, then stops
		//================================================
	case 5:

		GyroSonar_moveV2(0,SIDE_BACK,125,110,-60,true,true,CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(500);
		GyroTime_moveV2(1200,-30,true,false,REL);
		motor[motorA] = -50;
		wait1Msec(500);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,25,true,true,false,REL);

		GyroTimeS_moveV2(300,30,false,false,true,REL);
		GyroTimeS_moveV2(8000,15,true,true,true,REL);
		motor[RB_motor] = 0;
		motor[RF_motor] = 0;
		motor[LF_motor] = 0;
		motor[LB_motor] = 0;

		GyroTime45_V2(340,-30,true,true,REL,true);
		Gyro_TurnV2(42,15,REL);
		GyroTime_moveV2(800,-30,true,false,REL);

		motor[LB_motor] = 50;
		motor[LF_motor] = 50;
		wait1Msec(700);
		motor[RF_motor] = 50;
		wait1Msec(100);
		motor[LB_motor] = 0;
		motor[LF_motor] = 0;
		motor[RF_motor] = 0;
		wait1Msec(1000);
		servo[shoulder] = 0;
		wait1Msec(2000);

		wait1Msec(1500);
		servo[shoulder] = 0;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(800);

		break;
		//================================================
		// start on the right side of the blue dispencer and delivers to the right column
		//================================================
	case 6:

		GyroSonar_moveV2(0,SIDE_BACK,125,110,-60,true,true,CONSTANT);
		Gyro_TurnV2(42,-15,CONSTANT);
		wait1Msec(500);
		GyroTime_moveV2(1200,-30,true,false,REL);
		motor[motorA] = -50;
		wait1Msec(500);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,25,true,true,false,REL);
		GyroTimeS_moveV2(8000,-25,true,true,false,REL);

		GyroTimeS_moveV2(300,-30,false,false,true,REL);
		GyroTimeS_moveV2(8000,-15,true,true,true,REL);
		GyroTimeS_moveV2(350,-30,false,false,true,REL);
		motor[RB_motor] = 0;
		motor[RF_motor] = 0;
		motor[LF_motor] = 0;
		motor[LB_motor] = 0;

		Gyro_TurnV2(42,-15,REL);
		GyroTime_moveV2(800,-30,true,false,REL);
		GyroTimeS_moveV2(200,30,false,true,true,REL);

		motor[LB_motor] = 45;
		motor[LF_motor] = 45;
		wait1Msec(800);
		motor[LF_motor] = 0;
		motor[LB_motor] = 0;

		wait1Msec(1500);
		servo[shoulder] = 0;
		wait1Msec(2000);
		servo[wrist] = 130;
		wait1Msec(800);
		break;
	case 7:

		GyroTime_moveV2(800,-90,true,false,REL);
		GyroTime45_V2(400,90,false, true, REL, false);

		break;
	case 8:
		while(true)
		{}
		break;
	}
}
Example #29
0
task main()
{
  RegisterDriveMotors(LeftMid, LeftSides, RightMid, RightSides);
	InitializeTeleOP();

	waitForStart();

	StartTask(DriveTank);
	while(true)
	{
		if (joy2Btn(3) == 1)
		{
			servo[goalGrabber] = 150;
		}
		else if (joy2Btn(2) == 1)
		{
			servo[goalGrabber] = 45;
		}


		//Andymark motor on the right side of the robot has a 512 ticks per rotation value.
		//split the motors
		//arm raiser
		if (joy2Btn(8) == 1)
		{
			motor[Arm1] = 70;
			motor[Arm2] = 70;
		}
		else if (joy2Btn(7) == 1)
		{
			motor[Arm1] = 40;
			motor[Arm2] = -40;
		}
		else
		{
			motor[Arm1] = 0;
			motor[Arm2] = 0;
		}

		if (abs(joystick.joy2_y2) > 15)
		{
			servo[wrist] = joystick.joy2_y2;
		}

		//arm extender
		/*if (joy2Btn(8) == 1)
		{
			servo[leftExtender] = 255;
			servo[rightExtender] = 0;
		}
		else if (joy2Btn(7) == 1)
		{
			servo[leftExtender] = 0;
			servo[rightExtender] = 255;
		}
		else
		{
			servo[leftExtender] = 128;
			servo[rightExtender] = 128;
		}*/

		//tube code
		if (joy2Btn(4) == 1)
		{
			servo[ballCollecter] = 150;
		}
		else if (joy2Btn(1) == 1)
		{
			servo[ballCollecter] = 45;
		}


		/*while (joy2Btn(7) == 1 && nMotorEncoder[Arm] <= 1110)
		{
			motor[Arm] = 50;
		}
		nMotorEncoder[Arm] = 0;

		while(joy2Btn(6) == 1 && nMotorEncoder[Arm] >= -1110)
		{
			motor[Arm] = -50;

		}
		nMotorEncoder[Arm] = 0;*/
	}
}
Example #30
0
task main()
{
	waitForStart();

	initializeRobot();



	// The amount of time the robot...

	// ...moves forward at an angle.
	const int forwardTimeA	= 150;
	// ...turns to line up perpendicular to the center rack.
	const int turnTimeB		= 40;
	// ...drives up to the peg before lifting the lift up.
	const int forwardTimeC	= 155;
	// ...lifts the claw to put a ring on (row 2).
	const int liftTimeF		= 79;
	// ...moves forward, putting the ring onto the peg.
	const int forwardTimeG	= 65;
	// ...lowers its lift to get rid of the ring.
	const int liftTimeH		= 55;
	// ...backs up and gets ready to go to a dispenser.
	const int backwardTimeI	= 300;

	// ...turns to face parallel to the walls.
	const int turnTimeJ		= 40;
	// ...moves forward to align itself with a dispenser.
	const int forwardTimeK	= 100;
	// ...turns to face the dispenser.
	const int turnTimeL		= 80;
	// ...moves forward to be under the dispenser.
	const int forwardTimeM	= 50;


	Move_Forward	(forwardTimeA, g_AccurateMotorPower);
	Turn_Right		(turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower);
	Move_Forward	(forwardTimeC, g_AccurateMotorPower);
	Lift_Up			(liftTimeF, g_AccurateMotorPower);
	Move_Forward	(forwardTimeG, g_AccurateMotorPower);

	// Lift power is negative so that the lift goes DOWN, not UP.
	Lift_Down		(liftTimeH, g_AccurateMotorPower);
	Move_Backward	(backwardTimeI, g_AccurateMotorPower);

	// Turn power doesn't need to be negative (turns in-place).
	Turn_Right		(turnTimeJ, g_AccurateMotorPower, g_AccurateMotorPower);
	Move_Forward	(forwardTimeK, g_AccurateMotorPower);
	Turn_Right		(turnTimeL, g_AccurateMotorPower, g_AccurateMotorPower);
	Move_Forward	(forwardTimeM, g_AccurateMotorPower);


	while (true)
	{
		PlaySoundFile("moo.rso");
		while(bSoundActive == true)
		{
			Time_Wait(1);
		}
	}



}