task Arm1()
{
	int armPower;
	int armDirection;
	int armPower_raw;
	int limit = 75;
	int cut = 2.5;
	float Gain2 = 0.07;
	float Gain3 = 0.1;

	while (true)
	{
		getJoystickSettings(joystick);

		if (mode == 1)
			{
				armDirection = -1;
			}
			else if (mode == 2)
				{
					armDirection = 1;
				}
				else
					{
						armDirection = -1;
					}
		armPower_raw = joystick.joy1_y2;

		if (abs(armPower_raw) > deadband)
		{
			armPower = (armPower_raw - sgn(armPower_raw * deadband));

			armPower = ((armPower * Gain2) * (abs(armPower) * Gain3));

			if (armPower > limit)
			{
				armPower = limit;
			}
			else if (armPower < -limit)
			{
				armPower = -limit;
			}
			motor[Block_Arm] = (armDirection*armPower)/cut;
		}
		else
		{
			armPower = 0;
			motor[Block_Arm] = armPower;
		}

		if(joy1Btn( BTN_RB ))
		{
			if (ServoValue[Arm] == 0) //If the arm servo is open
			{
				servo[Arm] = 200; //Closes arm servo

			}
			if(ServoValue[Arm] == 200)
			{
				servo[Arm] = 0; //
			}
		}
if(joy1Btn(BTN_RT))
{
	motor[Right_Side] = -40;
	motor[Left_Side] = 40;
	motor[Arm] = 60;
	wait10Msec(100);
	motor[Right_Side] = 0;
	motor[Left_Side] = 0;
	motor[Arm] = 0;
}
	}
}
Exemplo n.º 2
0
task main()
{
		waitForStart();
	 	initializeRobot();
	 	initialize_gyro();
		StartTask(process_gyro);
	  motor[finger] = 40;

	  int irSensorVal;

	  // move forward, turn 45 degrees, move forward
	  move(1, 24, 2000);
	  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(1, 45, 4000);
  			wait10Msec(100);
  			turngyro_left(83.0, 30);
  			wait10Msec(100);
  			//move(1, 0.5, 2000);
  			nMotorEncoder[arm] = 0;
 				wait1Msec(50);

  			while(nMotorEncoder[arm] < 1600)
  				{
 	  				motor[arm] = 50;
  				}
				motor[arm] = 0;
  			wait1Msec(1500);
  			motor[finger] = -40;
  			wait1Msec(750);
  			motor[finger] = 0;
  			break;

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

  	case 2:
  		wait10Msec(100);
  		move(-1, 2, 1000);
  		turngyro_left(-45.0, 50);
  		wait10Msec(50);
  		ClearTimer(T1);
  		nMotorEncoder[RightMotor] = 0;
  		nMotorEncoder[LeftMotor] = 0;

  		/*while(sensorValue[sonic] < 30)
  		{
  		 	if(irSensorVal < 2)
  		 	{
  		 		motor[RightMotor] = -30;
  		 		motor[LeftMotor] = 30;
  		 	}
		 		if(irSensorVal == 2)
		 		{
		 			motor[RightMotor] = 30;
  		 		motor[LeftMotor] = 30;
		 		}
		 		if(irSensorVal > 2)
		 		{
		 			motor[RightMotor] = 30;
  		 		motor[LeftMotor] = -30;
		 		}
  		}
  		*/
			move(1, 56, 4000);
    	ClearTimer(T1);
 		  move(-1, 12.5, 2000);
  		nMotorEncoder[arm] = 0;
 			wait1Msec(50);

  		while(nMotorEncoder[arm] < 1600)
  			{
 	  			motor[arm] = 50;
  			}
			motor[arm] = 0;
  		wait1Msec(1500);
  		motor[finger] = -40;
  		wait1Msec(750);
  		motor[finger] = 0;
  		break;

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

  	case 3:
  			move(1, 43, 2000);
  			wait10Msec(100);
  			turngyro_left(-90.0, 50);
  			wait10Msec(100);
  			move(1, 2, 1000);
  			nMotorEncoder[arm] = 0;
 				wait1Msec(50);
  			while(nMotorEncoder[arm] < 1500)
  				{
 	  				motor[arm] = 50;
  				}
				motor[arm] = 0;
  			wait1Msec(1500);
  			motor[finger] = -40;
  			wait1Msec(750);
  			motor[finger] = 0;
  			break;
  	}

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

        /*
        if (irSensorVal == 4)
        {
        		wait1Msec(500);
        		motor[Leftmotor] = 50;
            motor[Rightmotor] = 50;
            wait1Msec(500);

            PlaySound(soundLowBuzz);
        }

        if (irSensorVal == 3)
        {
       			motor[Leftmotor] = 30;
            motor[Rightmotor] = 60;
            PlaySound(soundFastUpwardTones)
        }

        if (irSensorVal == 5)
        {
       			motor[Leftmotor] = 30;
            motor[Rightmotor] = 60;
            PlaySound(soundFastUpwardTones);
        }

        if (irSensorVal < 3)
        {
       			motor[Leftmotor] = -50;
            motor[Rightmotor] = 50;
            //wait1Msec(200);

            PlaySound(soundFastUpwardTones);
        }

        if (irSensorVal > 5)
        {
        		motor[Leftmotor] = 50;
            motor[Rightmotor] = -50;
           	//wait1Msec(200);

            PlaySound(soundBeepBeep);
        }
        //else
        //{

        //}
        wait1Msec(50);


        //motor[LeftMotor] = -50;
        //motor[RightMotor] = -50;
        */

}
Exemplo n.º 3
0
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
	initializeRobot();
	while(true)
	{
		bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase.
		eraseDisplay();
		//	WriteShort(hFileHandle, nIoResult, nParm);


		if ( externalBatteryAvg < 0)

		{
		while (externalBatteryAvg < 0)
		{
			eraseDisplay();
			nxtDisplayTextLine(1, "Ext Batt: OFF");       //External battery is off or not connected
			nxtDisplayCenteredBigTextLine(1, "What!?");
			nxtDisplayTextLine(3, "My name is,");
			nxtDisplayTextLine(4, "Iego Montoya,");
			nxtDisplayTextLine(5, "you did not turn");
			nxtDisplayTextLine(6, "on the robot");
			nxtDisplayTextLine(7, "NOW YOU DIE");
			nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!");
			PlayImmediateTone(600, 20);
			PlayImmediateTone(400, 20);
			wait10Msec(20);
		}
		}
		else
		{
			nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);
		}
		while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7)
		{
			eraseDisplay();

			nxtDisplayBigTextLine(6, "Battery");
			nxtScrollText("poopheads");
			nxtScrollText("You didnt change the battery you shmuts!");
			nxtScrollText("the battery you");
			nxtScrollText("shmuts!");
			PlayImmediateTone(600, 70);
			PlayImmediateTone(400, 70);
			wait10Msec(20);
		}

		nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000);   // Display NXT Battery Voltage
		nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]);
		nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1));
		nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]);
		nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]);


		wait10Msec(20);
	}//End of While
	//Actuation tests.
	motor[R_Motor] = 100;
	wait10Msec(30);

	motor[R_Motor] = 0;
	wait10Msec(30);

	motor[L_Motor] = 100;
	wait10Msec(30);

	motor[L_Motor] = 0;
	wait10Msec(30);

	motor[Flag_Raiser] = 100;
	wait10Msec(30);

	motor[Flag_Raiser] = 0;
	wait10Msec(20);

	motor[Winch] = 100;
	wait10Msec(30);

	motor[Winch] = 0;
	wait10Msec(30);

	motor[Arm] = 100;
	wait10Msec(30);

	motor[Arm] = 0;
	wait10Msec(30);

	motor[Block_Sweep] = 100;
	wait10Msec(30);

	motor[Block_Sweep] = 0;
	wait10Msec(30);

	servo[Bucket_Release] = 100;
	wait10Msec(30);

	servo[Bucket_Release] = 0;
	wait10Msec(30);

	servo[Spring_Release] = 100;
	wait10Msec(30);

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

	servo[IRS_1] = 100;
	wait10Msec(30);

	servo[IRS_1] = 0;
	wait10Msec(30);

}
Exemplo n.º 4
0
 //=============================================================================
// Function: Move()
//
// Description:
//    This function will drive the robot the distance specified in "distance".
//
// Parameters: distance = distance in inches, Positive is forward,
//                        Negative is backward
//=============================================================================
void Move( float distance, float max_speed )
{
		nMotorPIDSpeedCtrl[B_L]= mtrSpeedReg;
		nMotorPIDSpeedCtrl[B_R]= mtrSpeedReg;
		nMotorPIDSpeedCtrl[F_L]= mtrSpeedReg;
		nMotorPIDSpeedCtrl[F_R]= mtrSpeedReg;
	nMaxRegulatedSpeed12V = 1000;
  float COUNTS_PER_INCH = 103.55;
  int countDistance;
  int direction = 1;
  int motorSpeed = 0;
  int MAX_ENCODER_SPEED  = 50;
  int cut = 3;
	float Sum_left;
	float Sum_right;
	float RestofDst = 0.75;

  // Determine direction. Set to negative for backwards
  if( distance < 0 )
  {
    direction = -1;
  }
  if( distance <= 5 )
  {
  	motorSpeed = MAX_ENCODER_SPEED/cut;
  }
  // Calculate the distance to travel in encoder counts
  countDistance = abs(distance) * COUNTS_PER_INCH;

  // remove the backlash and freeplay from the motors before zeroing the encoders by commanding
  // a very low power for a short time

  motorSpeed = 5;
  motor[B_L] = direction * motorSpeed;
  motor[B_R] = direction * motorSpeed;
  motor[F_L] = direction * motorSpeed;
  motor[F_R] = direction * motorSpeed;
  wait1Msec(10);

  if ( (int)(abs(max_speed)) < MAX_ENCODER_SPEED)
  	{
  		MAX_ENCODER_SPEED = (int)max_speed;
    }

  // reset the motor encoders to zero
  nMotorEncoder[F_L] = 0;
  nMotorEncoder[F_R] = 0;
  nMotorEncoder[F_L] = 0;
  nMotorEncoder[F_R] = 0;
//  nMotorEncoderTarget[F_L] = countDistance;
 // nMotorEncoderTarget[F_R] = countDistance;

  motor[B_L] = direction * MAX_ENCODER_SPEED;
  motor[B_R] = direction * MAX_ENCODER_SPEED;
  motor[F_L] = direction * MAX_ENCODER_SPEED;
  motor[F_R] = direction * MAX_ENCODER_SPEED;


  while (abs(nMotorEncoder[F_R]) < countDistance && abs(nMotorEncoder[F_L])  <  countDistance && abs(nMotorEncoder[B_L])<  countDistance && abs(nMotorEncoder[B_R])<  countDistance)
  { //while the encoder value is less than the distance it needs to reach
    wait10Msec(25);
    Sum_left = (abs(nMotorEncoder[F_L]) + abs(nMotorEncoder[B_L])); //Ads up the encodr values on the left and right to compare them
    Sum_right = (abs(nMotorEncoder[F_R]) + abs(nMotorEncoder[B_R]));

    /*if (Sum_left > Sum_right)
    {
    	motor[F_R] = direction * MAX_ENCODER_SPEED + 5;
    	motor[B_R] = direction * MAX_ENCODER_SPEED + 5;
  }
    	else if (Sum_left < Sum_right)
    	{
    		motor[F_L] = direction * MAX_ENCODER_SPEED + 5;
    		motor[B_L] = direction * MAX_ENCODER_SPEED + 5;
  }*/
    if (abs(nMotorEncoder[F_R]) >= countDistance * RestofDst && abs(nMotorEncoder[F_L])  >=  countDistance * RestofDst && abs(nMotorEncoder[B_L]) >= countDistance * RestofDst && abs(nMotorEncoder[B_R]) >=  countDistance * RestofDst)
    { //If the encoder values becomes greater than a certain percentage of the total value needed to reach the desired distance.
        motor[B_L] = MAX_ENCODER_SPEED/cut; //the motor value will continue to be cut until the distance is reached.
  	  	motor[B_R] = MAX_ENCODER_SPEED/cut;
  		  motor[F_L] = MAX_ENCODER_SPEED/cut;
        motor[F_R] = MAX_ENCODER_SPEED/cut;
    }


   }

  motor[B_L] = -direction*20;
  motor[B_R] = -direction*20;
  motor[F_L] = -direction*20;
  motor[F_R] = -direction*20;

  wait10Msec(10);

  motor[B_L] = 0;
  motor[B_R] = 0;
  motor[F_L] = 0;
  motor[F_R] = 0;

  // reset the back motor encoders to zero
  nMotorEncoder[F_L] = 0;
  nMotorEncoder[F_R] = 0;
  nMotorEncoder[B_L] = 0;
  nMotorEncoder[B_R] = 0;

} // End of Move()
task main() {
  float temp;
  tLEGOTMPAccuracy accuracy;
  string strAcc;

  nxtDisplayCenteredTextLine(0, "LEGO");
  nxtDisplayCenteredBigTextLine(1, "Temp");
  nxtDisplayCenteredTextLine(3, "Test 1");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);
  eraseDisplay();


  // Setup the sensor for continuous mode
  LEGOTMPsetContinuous(LEGOTMP);

  //setting minimum accuracy
  accuracy = A_MIN;
  if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
    nxtDisplayTextLine(0, "Error setAccuracy");
    wait1Msec(5000);
    StopAllTasks();
  }

  //reads the current accuracy of the sensor
  if (!LEGOTMPreadAccuracy(LEGOTMP, accuracy)) {
    nxtDisplayTextLine(0, "Error readAccuracy");
    wait1Msec(5000);
    StopAllTasks();
  }

  accuracyToString(accuracy, strAcc);
  nxtDisplayTextLine(0, "Accuracy: %s", strAcc);

  //loop to read temp
  while (true) {
    switch(nNxtButtonPressed) {
      // If the left button is pressed, decrease the accuracy
      case kLeftButton:
        switch(accuracy) {
          case A_MIN:   accuracy = A_MAX;   break;
          case A_MEAN1: accuracy = A_MIN;   break;
          case A_MEAN2: accuracy = A_MEAN1; break;
          case A_MAX:   accuracy = A_MEAN2; break;
        }
        if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
          nxtDisplayTextLine(0, "Error setAccuracy");
          wait1Msec(5000);
          StopAllTasks();
        }
        accuracyToString(accuracy, strAcc);
        nxtDisplayTextLine(0, "Accuracy: %s", strAcc);

        // debounce the button
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;

      // If the right button is pressed, increase the accuracy
      case kRightButton:
        switch(accuracy) {
          case A_MIN:   accuracy = A_MEAN1; break;
          case A_MEAN1: accuracy = A_MEAN2; break;
          case A_MEAN2: accuracy = A_MAX;   break;
          case A_MAX:   accuracy = A_MIN;   break;
        }
        if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
          nxtDisplayTextLine(0, "Error setAccuracy");
          wait1Msec(5000);
          StopAllTasks();
        }
        accuracyToString(accuracy, strAcc);
        nxtDisplayTextLine(0, "Accuracy: %s", strAcc);

        // debounce the button
        while (nNxtButtonPressed != kNoButton) EndTimeSlice();
        break;
    }
    if (!LEGOTMPreadTemp(LEGOTMP, temp)) {
      eraseDisplay();
      nxtDisplayTextLine(0, "Temp reading pb");
      wait10Msec(100);
      StopAllTasks();
    }

    nxtDisplayCenteredBigTextLine(2, "Temp:");
    switch(accuracy) {
      case A_MIN:   nxtDisplayCenteredBigTextLine(4, "%4.1f", temp); break;
      case A_MEAN1: nxtDisplayCenteredBigTextLine(4, "%5.2f", temp); break;
      case A_MEAN2: nxtDisplayCenteredBigTextLine(4, "%6.3f", temp); break;
      case A_MAX:   nxtDisplayCenteredBigTextLine(4, "%7.4f", temp); break;
    }
  }
}
Exemplo n.º 6
0
task main()
{
	waitForStart();
	moveForward(0.15, 70);
	wait10Msec(50);
	stopMotors();
	wait10Msec(30);
	rightTwoWheelTurn(45, 40);
	wait10Msec(70);
	stopMotors();
	wait10Msec(30);
	//robot moves backward, so it will be able to sense the ir beacon underneath first bucket
	/*moveBackward(0.25, 80);
	wait10Msec(50);*/
	moveForward(6, 100);
	wait10Msec(50);
	stopMotors();
	wait10Msec(30);
	//keep going forward until ir sensor senses ir beacon
	StartTask(irRightTesting);
	wait10Msec(1400);
	//wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum
	while (SensorValue[sonarSensor] < 80)
	{
		motor[frontLeftMotor] = 100;
		motor[frontRightMotor] = -100;
		motor[backRightMotor] = 100;
		motor[backLeftMotor] = 100;
	}
	stopMotors();
	wait10Msec(10);
	moveForward(1.5, 90);
	wait10Msec(50);
	leftTwoWheelTurn(48, 85);
	wait10Msec(40);
	moveForward(30, 90);
	wait10Msec(50);
	leftTwoWheelTurn(42, 85);
	wait10Msec(30);
	moveForward(32, 90);
	wait10Msec(50);
	leftTwoWheelTurn(59, 85);
	wait10Msec(70);
	moveForward(44, 80);
	wait10Msec(50);
	//robot is parked in the middle of the ramp
}
Exemplo n.º 7
0
void ProgrammingSkillsAutonomous()
{
  armPosition = SensorValue[ArmPoten];
  SensorValue[Gyroscope] = 0;
  StartTask(KeepArmInPosition);

// A: Pick up stack at intersection
  armPosition = stackHeight;
  Intake(127);
  wait10Msec(40);
  Forward(45, 35, "none");

// B: Forward on line to doubler, pick up doubler, stop at intersection
  ForwardOnLine(45, 50, "none");
  Intake(0);
  ForwardOnLine(45, 100, "none");
  ForwardOnLine(45, 40, "none");  ///

  Intake(127);
  armPosition = minArm + 20;

  Forward(55, 100, "Intersection");
  StopMoving();
  armPosition = minArm + 100;
  Backward(45, 10, "Intersection"):
  Backward(45, 7, "none"):
  StopMoving();

// C: Turn 90° left to face 30" goal, forward, score doubler + 5 objects in 30" goal
  TurnLeftDegrees(91);
  Intake(0);
  armPosition = maxArm + 50;
  wait10Msec(100);
  Forward(50, 33.5, "none");
  StopMoving();
  Intake(-127);
  wait10Msec(60);
  Intake(-65);
  wait10Msec(60);
  Intake(-120);
  wait10Msec(40);
  Intake(-65);
  wait10Msec(60);
  Intake(-120);
  wait10Msec(80);
  // Intake(-127);
  // wait10Msec(250);
  Intake(0);

// D: Back up, lower arm, forward and pick up stack in front of 30" goal         //start of what grace programmed
  Backward(50, 50, "Intersection");
  wait10Msec(5);
  StopMoving();
  wait10Msec(20);
  armPosition = stackHeight + 20;
  wait10Msec(90);
  Intake(127);
  Forward(35, 28, "none"):
  StopMoving();

// E: Back up, turn 90° right to face 11.5" goal, score 11.5" goal
  Backward(35, 40, "Intersection");
  Backward(35, 8, "none");
  StopMoving();
  TurnRightDegrees(90);
  Intake(0);
  armPosition = lowGoalHeight;
  wait10Msec(20);
  Forward(35, 16, "none");
  StopMoving();
  wait10Msec(10);
  Intake(-85);
  wait10Msec(50);
  Intake(-0);
  wait10Msec(20);
  Intake(-85);
  wait10Msec(70);
  Intake(0);

// F: Back up to intersection, turn 90° right, forward a lot, pick up stack, score stack on 11.5" goal

  Backward(85, 30, "none");
  armPosition = stackHeight;
  // Backward(85, 50, "none");
  // Backward(65, 35, "none");
  // Backward(45, 40, "none");
  Backward(85, 120, "none");
  Backward(35, 70, "Intersection");
  wait10Msec(1);
  StopMoving();
  TurnRightDegrees(90);
  ForwardOnLine(35, 20, "none");
  ForwardOnLine(45, 80, "none");

  Intake(127);
  ForwardOnLine(45, 53, "none");

  // armPosition = stackHeight;////


  Forward(45, 40, "Intersection");
  StopMoving();
  armPosition = lowGoalHeight + 40;
  wait10Msec(40);
  Forward(45, 25, "none");
  StopMoving();
  Intake(-100);
  wait10Msec(55);
  Intake(-0);
  wait10Msec(20);
  Intake(-85);
  wait10Msec(55);
  Intake(0);

// G: Back up, pick up stack in front of 11.5" goal, back up to intersection, turn 90° left
  Backward(55, 28, "none");
  StopMoving();
  armPosition = stackHeight + 20;
  Intake(127);
  Forward(45, 40, "none");
  StopMoving();
  Backward(55, 60, "Intersection");
  Backward(45, 5, "none");
  StopMoving();
  TurnLeftDegrees(90);
  Backward(85, 40, "none");
  StopMoving();
  Intake(0);

//======================================== PART 2 ===============================================================

  while (SensorValue[Bump] == 0) {}

// A2: Go forward
  SensorValue[Gyroscope] = 0;
  Forward(50, 30, "none");

// B2: Forward on line to doubler, pick up doubler, stop at intersection
  ForwardOnLine(45, 130, "none");

   // ForwardOnLine(45, 20, "none");///

  Intake(127);
  armPosition = minArm + 40;
  Forward(55, 100, "Intersection");
  StopMoving();
  armPosition = minArm + 120;
  Backward(45, 10, "Intersection"):
  Backward(45, 5, "none"):
  StopMoving();

// C2: Turn 90° right to face 30" goal, forward, score doubler + 5 objects in 30" goal
  Intake(0);
  TurnRightDegrees(91);
  // Intake(0);
  StartTask(KeepArmInPosition);
  armPosition = maxArm + 50;
  wait10Msec(90);
  Forward(40, 40.5, "none");
  StopMoving();
  Intake(-100);
  wait10Msec(60);
  Intake(-55);
  wait10Msec(90);
  Intake(-100);
  wait10Msec(50);
  Intake(-55);
  wait10Msec(60);
  Intake(0);

// D2: Back up, lower arm, forward and pick up stack in front of 30" goal
  Backward(55, 50, "Intersection");
  StopMoving();
  wait10Msec(20);
  armPosition = stackHeight;
  wait10Msec(100);
  Intake(127);
  Forward(45, 22, "none"):
  StopMoving();

// E2: Back up, turn 180° left to face 20" goal, forward and pick up 2 objects
  Backward(55, 50, "Intersection");
  StopMoving();
  TurnLeftDegrees(186);
  Intake(0);
  armPosition = stackHeight + 180;
  wait10Msec(10);
  Intake(127);
  Forward(55, 14, "none");
  StopMoving();
  wait10Msec(5);
  armPosition = stackHeight + 300;
  Backward(55, 6, "none");
  armPosition = minArm + 60;
  wait10Msec(5);
  Forward(55, 30, "none");
  Backward(55, 20, "none");
  StopMoving();

//F2: Raise arm to midgoal height, score 2 and 2 on 20" goals
  armPosition = midGoalHeight + 40;
  wait10Msec(100);
  Intake(0);
  Forward(55, 13, "none");
  StopMoving();
  wait10Msec(10);
  Intake(-95);
  wait10Msec(300);

  /*
  wait10Msec(80);
  Intake(100);
  Forward(55, 27, "none");
  StopMoving();
  Intake(-95);
  wait10Msec(160);
  */

  StopTask(KeepArmInPosition);

}
task main()
{
	while(SensorValue[bumperLeft] == 0)
	{
	}
	StartTask(intakeStart);
	//StartTask(armcontrol);
	//armPos = 1000;
	// Start from hanging zone (Top left)
	ClearTimer(T4);
	moveSecondTierUp(127,450);
	moveSecondTierDown(127,0);
	intake = 1;
	motor[secondTier]=-127;
	wait10Msec(50);
	motor[secondTier]=0;
	moveStraightDistance(127,200);
	stopPid(0.6,0.3);
	wait10Msec(10);
	moveStraightDistance(30, 200);
	stopPid(0.6,0.3);
	wait10Msec(50);
	turnRight(127,600);
	moveFirstTierUp(127,300);
	motor[firstTierLeft]=20;
	motor[firstTierRight]=20;
	intake = 0;
	moveStraightTime(127, 2000);
	moveFirstTierDown(127, 0);
	// Continue from middle zone (Left)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127,350);
	alignFoward(127);
	stopPid(0.6,0.3);
	moveStraightDistance(100,50);
	stopPid(0.6,0.3);
	wait10Msec(10);
	turnRight(100,280);
	stopTurn(0.6,0.3);
  moveStraightDistance(100,1400);
	stopPid(0.6,0.3);
	moveFirstTierUp(127,1200);
	motor[firstTierLeft] = 10;
	motor[firstTierRight] = 10;
	moveStraightDistance(100,200);
	turnLeft(50,30);
	stopTurn(0.6,0.3);
	moveStraightTime(50,500);
	intake = -1;
	wait10Msec(300);
	intake = 0;
	moveStraightDistance(-50,250);
	moveFirstTierDown(127,50);
	turnLeft(127,40);
	moveStraightDistance(-100, 1300);

	// Continue from Middle Zone (Left)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127, 1500);
	alignFoward(127);
	intake = 1;
	moveStraightDistance(40, 500);
	wait10Msec(50);
	intake = 0;

	// Continue from Middle Zone (Right)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(-127,300);
	moveFirstTierUp(127,400);
	motor[firstTierLeft]=20;
	motor[firstTierRight]=20;
	moveStraightDistance(127,1800);
	moveFirstTierDown(127,400);

	// Continue from Hanging Zone (Top right)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	intake = 1;
	moveStraightDistance(127,200);
	stopPid(0.6,0.3);
	wait10Msec(10);
	moveStraightDistance(30, 200);
	stopPid(0.6,0.3);
	wait10Msec(170);
	intake = 0;
	turnLeft(127,650);
	intake = 1;
	moveFirstTierUp(127,400);
	motor[firstTierLeft]=10;
	motor[firstTierRight]=10;
	intake = 0;
	moveStraightDistance(127, 1300);
	moveFirstTierUp(127, 500);
	motor[firstTierLeft]=10;
	motor[firstTierRight]=10;

	// Continue from Middle Zone (Right)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127,350);
	stopPid(0.6,0.3);
	alignFoward(127);
	moveStraightDistance(127,100);
	stopPid(0.6,0.3);
	wait10Msec(10);
	turnLeft(127,380);
	moveStraightDistance(127,1300);
	stopPid(0.6,0.3);
	moveFirstTierUp(127,1100);
	moveStraightDistance(100,100);
	motor[firstTierLeft] = 10;
	motor[firstTierRight] = 10;
	turnRight(50,30);
	moveStraightDistance(50,200);
	intake = -1;
	wait10Msec(300);
	intake = 0;
	moveStraightDistance(-50,250);
	moveFirstTierDown(127,50);
	turnRight(127,50);
	moveStraightDistance(-50, 1300);

	// Continue from Middle Zone (Right)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveFirstTierUp(127,1100);
	motor[firstTierLeft] = 20;
	motor[firstTierRight] = 20;
	moveStraightDistance(127,800);
	moveStraightDistance(-127,600);

	// Continue from Middle Zone (Right)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127,1200);
	moveStraightDistance(-127,500);
	turnRight(127,350);
	moveStraightDistance(127,1500);

	// Continue from Middle Zone (Left)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127,800);
	moveStraightDistance(-127,600);

	// Continue from Middle Zone (Left)
	while (SensorValue[bumperLeft] == 0)
	{
	}
	moveStraightDistance(127,1000);
	moveStraightDistance(-127,1100);
}
Exemplo n.º 9
0
task main()
{
  accelerate(50);
  wait10Msec(100);
}
Exemplo n.º 10
0
task main()
{
	while(SensorValue[irsensor] != 7)
	{

		DriveMotorsForward();
	}

	wait10Msec(15);

	StopDriveMotors();

	wait10Msec(10);

	TurnRight();

	wait10Msec(83);

	StopDriveMotors();

	wait10Msec(5);

	if(SensorValue(ultrasonic) < 30)
	{
		while(SensorValue(ultrasonic) < 30)
		{
			DriveMotorsBackward();
		}
	}

	StopDriveMotors();


	motor[liftassembly1] = 100;
	wait10Msec(600);
	motor[liftassembly1] = 0;


	if(SensorValue(ultrasonic) > 25)
	{
		while(SensorValue(ultrasonic) > 25)
		{
			DriveMotorsForward();
		}
	}
	StopDriveMotors();


	motor[bucketwrist] = -100;
	wait10Msec(500);
	motor[bucketwrist] = 0;

	DriveMotorsBackward();

	wait10Msec(25);

	StopDriveMotors();
	wait10Msec(5);

	motor[liftassembly1] = -100;
	wait10Msec(325);

	motor[liftassembly1] = 0;

	wait10Msec(30);

	TurnRight();
	wait1Msec(1000);

	StopDriveMotors();
	wait10Msec(30);

	while(SensorValue(ultrasonic) > 40)
	{
		DriveMotorsForward();
	}
	StopDriveMotors();
	wait10Msec(30);

	TurnLeft();        //Turn to be perpendicular to ramp
	wait10Msec(75);
	StopDriveMotors();
	wait10Msec(30);

	DriveMotorsForward(); // Drive alongside  ramp
	wait10Msec(200);
	StopDriveMotors();
	wait10Msec(30);

	TurnLeft();           // turn to face ramp
	wait10Msec(107);
	StopDriveMotors();
	wait10Msec(30);

	DriveMotorsForward();  //Drive onto ramp (hopefully)
	wait10Msec(200);
	StopDriveMotors();




}
Exemplo n.º 11
0
task main()
{
	waitForStart();
	initializeRobot();
	initialize_gyro();
	StartTask(process_gyro);

	int irSensorVal;

	move(1, 24, 2000);
	wait10Msec(50);
	move(1, 0, 1000);

	ClearTimer(T1);
  wait10Msec(50);

  irSensorVal = SensorValue[IR];
  nxtDisplayBigTextLine(2, "IR: %d", irSensorVal);

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

	switch(irSensorVal)
	{
	case 1:
		turngyro_left(-90.0, 35);
		wait10Msec(100);

		move(1, 45, 4000);
		wait10Msec(100);
		turngyro_left(83.0, 30);
		wait10Msec(100);
		move(1, 10, 1000);
		motor[RightMotor] = 50;
		wait10Msec(400);
		move(-1, 12, 2000);

		while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/)
		{
			motor[lifter] = 30;
		}
		move(1, 2, 300);
		while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/)
		{
			motor[lifter] = -30;
		}
		break;

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

	case 2:
		wait10Msec(100);
		move(-1, 2, 1000);
		turngyro_left(-50.0, 50);
		wait10Msec(50);

		while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/)
		{
			motor[lifter] = 30;
		}
		move(1, 2, 300);
		while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/)
		{
			motor[lifter] = -30;
		}
/*
		ClearTimer(T1);
		nMotorEncoder[RightMotor] = 0;
		nMotorEncoder[LeftMotor] = 0;
		move(1, 56, 4000);
		ClearTimer(T1);
		move(-1, 12.5, 2000);
		nMotorEncoder[arm] = 0;
		wait1Msec(50);

		while(nMotorEncoder[arm] < 1600)
		{
			motor[arm] = 50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = -40;
		wait1Msec(750);
		motor[finger] = 0;
		while(nMotorEncoder[arm] > 800)
		{
			motor[arm] = -50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = 0;*/
		break;

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

	case 3:
		move(1, 43, 2000);
		wait10Msec(100);
		turngyro_left(-100.0, 50);
		wait10Msec(100);
		move(1, 10, 1000);
		motor[LeftMotor] = 50;
		wait10Msec(400);
		move(-1, 12, 2000);

		while(nMotorEncoderTarget(lifter) /*< LIFTER_UP*/)
		{
			motor[lifter] = 30;
		}
		move(1, 2, 300);
		while(nMotorEncoderTarget(lifter) /*< LIFTER_DOWN*/)
		{
			motor[lifter] = -30;
		}
		/*
		nMotorEncoder[arm] = 0;
		wait1Msec(50);
		while(nMotorEncoder[arm] < 1500)
		{
			motor[arm] = 50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = -40;
		wait1Msec(750);
		motor[finger] = 0;
		while(nMotorEncoder[arm] > 800)
		{
			motor[arm] = -50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = 0;*/
		break;
	}
}
Exemplo n.º 12
0
void chill(){
  motor[rightWheel] = 0;
  motor[leftWheel] = 0;
  wait10Msec(100);
}
Exemplo n.º 13
0
void Right90deg(){
  motor[rightWheel] = TurnSpeed*-1;
  motor[leftWheel] = TurnSpeed;
  angle = (angle-90 )% 360;
  wait10Msec(TurnTime);
}
Exemplo n.º 14
0
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    findBeacon
//
// This routine uses the IR seeker sensor to orient the robot based on the IR beacon
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int findBeacon(int iPos, string iSide)
{
	int SensorVal1;
	int BeaconPos;

	//First we must turn the robot until the beacon is in front of us
	if (strcmp(iSide, "left")== 0)
	{
		nxtDisplayCenteredTextLine(5, "Left");

		while (SensorValue[irSeeker] < 7 && ServoValue[IR] < 255)
		{
			servo[IR] = ServoValue[IR] + 1;
		}
	}
	else
	{
		nxtDisplayCenteredTextLine(5, "Right");

		while (SensorValue[irSeeker] > 3 && ServoValue[IR] > 0)
		{
			servo[IR] = ServoValue[IR] - 1;
		}
	}

	SensorVal1 = ServoValue[IR];

	BeaconPos = ServoValue[IR];

	if (debugMode == ON)
	{
		eraseDisplay();
		nxtDisplayCenteredTextLine(1, "Beacon Found");
		nxtDisplayTextLine(3, "%i",BeaconPos);
		wait10Msec (10000);
	}

	if (iPos == 1)
	{
		if (strcmp(iSide, "left")== 0)
		{
			if (BeaconPos < 78)
			{
				return 1;
			}
			else if (BeaconPos < 97)
			{
				return 2;
			}
			else
			{
				return 3;
			}
		}
		else
		{
			if (BeaconPos < 94)
			{
				return 3;
			}
			else if (BeaconPos < 114)
			{
				return 2;
			}
			else
			{
				return 1;
			}
		}
	}
	else if (iPos == 2)
	{
		if (strcmp(iSide, "left")== 0)
		{
			if (BeaconPos < 91)
			{
				return 1;
			}
			else if (BeaconPos < 114)
			{
				return 2;
			}
			else
			{
				return 3;
			}
		}
		else
		{
			if (BeaconPos < 78)
			{
				return 3;
			}
			else if (BeaconPos < 103)
			{
				return 2;
			}
			else
			{
				return 1;
			}
		}
	}
	else
	{
		return 1;
	}
}
Exemplo n.º 15
0
void deploy()
{
	intake(-1);
	wait10Msec(70);
	intake(0);
}
Exemplo n.º 16
0
task main()
{
	waitForStart();

	wait1Msec(1450);

	MotorEncoderTarget(2, 11, 11, 65, 10);

	if (SensorValue[IRSeeker] > 2)
	{
		MotorEncoderTarget(2, 20, 20, 65, 10);

		Temp1++;

		if (SensorValue[IRSeeker] > 2)
		{
			MotorEncoderTarget(2, 21, 21, 65, 10);

			Temp1++;
		}

	}
	wait10Msec(20);

	MotorEncoderTarget(1, -2100, 2100, 60, 30);
	//MotorEncoderTarget(1, (-2000 + Temp1*50), (1950 - Temp1*50), 60, 30);

	if(Temp1 == 1)
	{
		MoveLift(1, 3000, 30);
	}
	else
	{
		MoveLift(1, 600, 30);
	}

	MotorEncoderTarget(2, 30, 30, 70, 30);
	LineCheck(7 * 115);
	motor[motorL] = 0;
	motor[motorR] = 0;

	if(Temp1 == 1)
	{
		MoveLift(2, 1200, 30);
	}
	else
	{
		MoveLift(2, 550, 30);
	}

	MotorEncoderTarget(2, -36, -36, 80, 10);

	MotorEncoderTarget(1, 2100, -2100, 80, 10);

	switch (Temp1)
	{
		case 0:
			MotorEncoderTarget(2, -10, -10, 80, 10);
			break;
		case 1:
			MotorEncoderTarget(2, -30, -30, 80, 10);
			break;
		case 2:
			MotorEncoderTarget(2, -55, -55, 80, 10);
			break;
	}

}
Exemplo n.º 17
0
void progSkills()
{
	//while (SensorValue[bumper] == 0) {}
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
	baseStop();
	// Deploy
	deploy();

	moveLiftAuto(1,3100);
	moveLift(20);
	SensorValue[rightBackS] = 0;
	while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);
	baseStop();
	/*
	moveStraight(127);
	wait1Msec(200);
	moveStraight(-127);
	wait1Msec(200);
	*/
	moveStraight(127);
	wait1Msec(200);
	intake(-1);
	moveStraight(	-127);
	wait1Msec(200);
	baseStop();

	//intake(-1);
	wait1Msec(2000);

	moveStraight(50);
	wait1Msec(100);
	baseStop();

	intake(1);
	moveLift(-30);
	wait1Msec(500);
	intake(0);

	liftStop();
	moveStraight(-40);
	wait1Msec(200);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)
	{
		while (SensorValue[rightLiftS] < 4000)
		{
			moveLift(-70);
			turnOnSpot(2,90);
		}
		liftStop();
	}
	baseStop();
	intake(1);
	moveStraight(70);
	wait1Msec(1000);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
	{
		turnOnSpot(2,90);
	}
	baseStop();
	moveStraight(127);
	wait1Msec(2000);
	baseStop();

	while(SensorValue[bumper] == 0) {}
	intake(-1);

	while(SensorValue[bumper] == 0) {}
	intake(0);
	// Raise Lift
	// Output

	while(SensorValue[bumper] == 0) {}
	intake (1);
	moveStraight(127);
	wait10Msec(50);
	turnOnSpot(1,60);
	wait10Msec(30);
	turnOnSpot(2,60);
	wait10Msec(30);
	baseStop();

	SensorValue[leftBackS] = 0;
	SensorValue[rightBackS] = 0;
	while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
	{
		turnOnSpot(1,90);
	}
	intake(-1);
	wait10Msec(100);
}
task main()
{
	{
	initializeRobot();

	bFloatDuringInactiveMotorPWM = false;
	nxtDisplayTextLine(2, "Waiting for start");
	waitForStart();		// wait for start of tele-op phase
	nxtDisplayTextLine(2, "starting");
	StartTask( drive );
	StartTask( Arm );

	float watchedmessage;
	//int watch = 1;
	while(true)
	{
		watchedmessage = ntotalMessageCount; // grabs the number of Messages and names it watchedMessage

		wait10Msec(25); // give the Messagecount time to change it changes every 50ms

		if(watchedmessage == ntotalMessageCount) // checks to see if the Message change if has not it is Disconnected
		{                // if Disconnected it runs this
			motor[mtr_S1_C1_1] = 0; // stops motor
			motor[mtr_S1_C1_2] = 0;
			motor[mtr_S1_C2_1] = 0;
			motor[mtr_S1_C2_2] = 0;
			motor[mtr_S1_C3_1] = 0;
			motor[mtr_S1_C3_2] = 0;
			/*motor[mtr_S1_C4_1] = 0;
			motor[mtr_S1_C4_2] = 0;*/



			StopTask(Arm); //stops task robot control task
			StopTask(drive);

			wait1Msec(10); //gives time to spot

			StartTask(Arm); //restart task incase of reconnection
			StartTask(drive);

			nxtDisplayTextLine(2, "DISCONNECTED"); //displays DISCONNECTED to tell you if you are Disconnected
		}
		else//if the Message change you are connected
		{

			 nxtDisplayTextLine(2, "CONNECTED");//displays CONNECTED to tell you if everything is working

		}
	}

}

	while (true)
	{
		getJoystickSettings(joystick);


		if (joy1Btn(BTN_RB))
			if (servo[srelease] < 100)
			{
				servo[srelease] = 245;
				wait10Msec(50);
			}
			else if (servo[srelease] > 100 /*|| joy1Btn(BTN_X)*/)
			{
				servo[srelease] = 0;
				wait10Msec(50);
			}

			if (joy1Btn(BTN_RT))
			if (servo[sdunk] < 100)
			{
				servo[sdunk] = 245;
				wait10Msec(50);
			}
			else if (servo[sdunk] > 100 /*|| joy1Btn(BTN_X)*/)
			{
				servo[sdunk] = 0;
				wait10Msec(50);
			}


		if (joy1Btn(BTN_X))
		{
		//	servo[srelease] = 255;
			motor[belt] = -100;
		}

		else if (joy1Btn(BTN_LB))
		{
		//	servo[srelease] = 255;
			motor[belt] = 100;
		}
		else if (!joy1Btn(BTN_LB) && !joy1Btn(BTN_X))
			motor[belt] = 0;

		if (joy1Btn(BTN_LT))
		{
			if (servo[stilt] == 0)
			{
				servo[stilt] = 35;
				wait10Msec(50);
			}
			else if (servo[stilt] == 35)
			{
				servo[stilt] = 0;
				wait10Msec(50);
			}
		}


	}
}
Exemplo n.º 19
0
task usercontrol()
{
  // StartTask(KeepArmInPosition);
  armPosition = SensorValue[ArmPoten];
  while (true)
  {
//  #########################################-  Arm and Intake -########################################################

    // if (!armPositionTaskActive) LiftArm(127 * vexRT[Btn5U] - 127 * vexRT[Btn5D]);                      //Main joystick Control
    // Intake(127 * vexRT[Btn6U] - 127 * vexRT[Btn6D]);

    // if (!armPositionTaskActive) LiftArm(vexRT[Ch3Xmtr2]);                                              //Partner joystick Control
    // Intake(vexRT[Ch2Xmtr2]);

    if (!armPositionTaskActive) LiftArm(vexRT[Ch3Xmtr2] + 127 * vexRT[Btn5U] - 127 * vexRT[Btn5D]);       //Dual Control
    Intake(vexRT[Ch2Xmtr2] + 127 * vexRT[Btn6U] - 127 * vexRT[Btn6D]);

//  ############################################-  Drive -##############################################################

	  motor[FrontLeft] = abs(vexRT[Ch3]+vexRT[Ch1]) > 10 ? vexRT[Ch3]+vexRT[Ch1] : 0;
	  motor[BackLeft] = abs(vexRT[Ch3]+vexRT[Ch1]) > 10 ? vexRT[Ch3]+vexRT[Ch1] : 0;
	  motor[FrontRight] = abs(-vexRT[Ch3]+vexRT[Ch1]) > 10 ? -vexRT[Ch3]+vexRT[Ch1] : 0;
	  motor[BackRight] = abs(-vexRT[Ch3]+vexRT[Ch1]) > 10 ? -vexRT[Ch3]+vexRT[Ch1] : 0;

//  ##########################################- Arm Presets -###########################################################

    if (vexRT[Btn5DXmtr2] == 1)
      MoveArmToPreset(stackHeight);
    if (vexRT[Btn5UXmtr2] == 1)
      MoveArmToPreset(lowGoalHeight);
    if (vexRT[Btn6UXmtr2] == 1)
      MoveArmToPreset(midGoalHeight);
		if (vexRT[Btn6DXmtr2] == 1)
      MoveArmToPreset(maxArm);

    if (vexRT[Btn8U] == 1)
      armPosition = maxArm;
    if (vexRT[Btn8L] == 1)
      armPosition = midGoalHeight;
    if (vexRT[Btn8R] == 1)
      armPosition = lowGoalHeight;
    if (vexRT[Btn8D] == 1)
      armPosition = minArm;

    if (abs(vexRT[Ch3Xmtr2]) > 20) // || time10(T2) > armPresetTimer)
    {
      StopTask(KeepArmInPosition);
      armPositionTaskActive = false;
    }


    if (vexRT[Btn7U] == 1) ProgrammingSkillsAutonomous();
    if (vexRT[Btn7D] == 1)
    {
      Forward(127, 40, "none");
		  VariableMove(127, 20, 70);
		  Forward(127, 120, "none");
		  StopMoving();
		  StartTask(StayInPosition);
		  wait10Msec(1500);
		  StopTask(StayInPosition);
    }
    if (vexRT[Btn7R] == 1) {TurnLeftDegrees(90); wait10Msec(50); }
    if (vexRT[Btn8L] == 1) {TurnRightDegrees(90); wait10Msec(50); }

  }
}
Exemplo n.º 20
0
sub MTASK_DOTASK(int MTASK_ID){
  switch (MTASK_ID)
  {
    //********
    case MT_DEFAULT:
	    wait1Msec(1);
	    break;

    //********
    case MT_BEEP:
	    PlayTone(200, 12);
	    wait10Msec(120);
	    break;

    //********
	  case MT_STOP_BUTTON:
	    if(nNxtButtonPressed==BT_ENTER)
	    {
	      int static TimeDif;
	      TimeDif=time10[T4];
	      while(nNxtButtonPressed==BT_ENTER){
	        if(time10[T4]-TimeDif>50)
	        {
    	      MV_StopMotors();
    	      ClearSounds();
	          PlaySound(soundBlip);
	          ESTADO_SET_TARGET(ST_WAIT);
	          break;
	        }
	      }
	    }
	    break;

	  //********
    case MT_ACCEL:
      static int LastTime;

      if(time10[T3]-LastTime >= 30)
      {
        HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);
        if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){
          if (ACCEL_Filter++ > 2){
            ACCEL_Rampa = true;
            ACCEL_Filter = 3;
          }
        }else{
          if (ACCEL_Filter-- < 2){
            ACCEL_Filter = 0;
            ACCEL_Rampa = false;
          }
        }
        LastTime=time10[T3];
      }

	    break;

	  //********
    case MT_TOQUE:
	    wait1Msec(1);
	    break;
  }
}
Exemplo n.º 21
0
task main()
{
	initializeRobot();

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

	servo[srelease] = 140;
	servo[sgrab] = 100;
	while (true)
	{
		getJoystickSettings(joystick);
///////////////////////////////////
//      Release Mechanism        //
///////////////////////////////////
		if (joy1Btn(BTN_RT))
		{
			if (servo[srelease] < 170)
			{
				servo[srelease] = 255;
				wait10Msec(50);
			}
			else if (servo[srelease] > 170)
			{
				servo[srelease] = 140;
				wait10Msec(50);
			}
		}

///////////////////////////////////
//         Goal Grabber          //
///////////////////////////////////
		if (joy1Btn(BTN_LT))
		{
			if (servo[sgrab] < 50)
			{
				servo[sgrab] = 100;
				wait10Msec(50);
			}
			else if (servo[sgrab] > 50)
			{
				servo[sgrab] = 40;
				wait10Msec(50);
			}
		}


///////////////////////////////////
//        Conveyor Belt          //
///////////////////////////////////
		if (joy1Btn(BTN_X))
		{
			motor[belt] = -100;
		}

		else if (joy1Btn(BTN_LB))
		{
			motor[belt] = 100;
		}
		else if (!joy1Btn(BTN_LB) && !joy1Btn(BTN_X))
			motor[belt] = 0;

	}
}
Exemplo n.º 22
0
//************************
// Sobe a Rampa
//************************
void F_STATE_SOBE()
{
  PlayTone(500,10);

  MTASK_SET_RUN(MT_ACCEL,MT_STOP_BUTTON);

  int USminDist = 999, USdist,USdelta,Lado1,Lado2;

  float Error = 0, vI = 0, vP = 0, vD = 0, kI = 0.3, kP = 1.5, kD = 1, lD=4;

  tByteArray USwall;

  MV_StopMotors();
  GARRA_V(PORT_ARD,GARRA_V_DESCE);
  wait10Msec(30);
  GARRA_V(PORT_ARD,GARRA_V_PARA);

  time1[T1]=0;

  while(ESTADO_IS_CURRENT())
  {
    //=====================================

    // Le arduino para pingar os ultrasons
    wait10Msec(3);
    ReadAllUS_short(PORT_ARD,USwall);

    // Filtra dados

    //// Seta as variaveis locais como locais
    //// para nao dar bug (alterar o valor na outra task)
    Lado1=USwall[0];
    Lado2=USwall[2];

    //// Acha a menor distancia que o robo fica das paredes
    //if(Lado1+Lado2<USminDist)USminDist=USdist;

    //// Filtra paredes (existe ou nao)
    if(Lado1>60)Lado1=60-Lado2;
    if(Lado2>60)Lado2=60-Lado1;

    // Calcula o Error
    vD =  time1[T1] * ((Lado1 - Lado2) - vD) / 1000;
    lD =  vD;
    vP =  (Lado1 - Lado2);
    vI += time1[T1] * vP / 1000;

    //Aplica constantes
    vP *= kP;
    vI *= kI;
    vD *= kD;

    if(vI>20)  vI = 20;
    if(vI<-20) vI = -20;

    Error = vP+
            vI-
            vD;

    time1[T1]=0;

    // Aplica erro aos motores
    motor[MA]=85 - Error;
    motor[MC]=85 + Error;

    //Mostra no display os valores P, I, D e Stering final (Error)
    eraseDisplay();
    nxtDisplayString(2,"P = %i", (int)vP);
    nxtDisplayString(3,"I = %i", (int)vI);
    nxtDisplayString(4,"D = %i", (int)vD);
    nxtDisplayBigStringAt((46 - Error)/2, 24, "|");

    //Se saiu da rampa com acceleracao, entra na sala
    if(!ACCEL_Rampa)ESTADO_SET_TARGET(ST_ENTRA);
    //=====================================
  }
}
Exemplo n.º 23
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;
		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
		wait1Msec(delayAutoStartValue);

		// INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE
		// move forward, turn 45 degrees, move forward
		move(1, 24, 2000);
		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(1, 45, 4000);
			wait10Msec(100);
			turngyro_left(83.0, 30);
			wait10Msec(100);
			//move(1, 0.5, 2000);
			wait1Msec(50);
			break;

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

		case 2:
			wait10Msec(100);
			move(-1, 2, 1000);
			turngyro_left(-45.0, 50);
			wait10Msec(50);
			ClearTimer(T1);
			nMotorEncoder[RightMotor] = 0;
			nMotorEncoder[LeftMotor] = 0;
			move(1, 56, 4000);
			ClearTimer(T1);
			move(-1, 12.5, 2000);
			wait1Msec(50);
			break;

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

		case 3:
			move(1, 43, 2000);
			wait10Msec(100);
			turngyro_left(-90.0, 50);
			wait10Msec(100);
			move(1, 2, 1000);
			nMotorEncoder[arm] = 0;
			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
		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
		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
		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;
		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
		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;
	default:	// Not a valid autonomous action
		break;
	}
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);
	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(30-5, 5000, 90, 90);
	turngyro_right(135, 40);
	move_forward(20-2, 5000, 90, 90);
	turngyro_left(90, 40);
	move_forward(10-2, 5000, 90, 90);
	turngyro_left(90, 40);
	move_backwards(10-2, 5000, 90, 90);

	break;

	servo[colorSensorServo] = colorServoDefault;
	wait10Msec(200);
	// All Done, time to stop all tasks
	StopAllTasks();
}	// End Main Bracket
task main()
{
	motor[port2]=127;
	wait10Msec(1000);
}
task autonomous()
{
	if(sensorValue(redBlueSwitch) < SWITCH){
		red = true;
	}
	else{
		red = false;
	}
	if(red){
		motor[hook] = -127;
		wait10Msec(60);
		motor[hook] = 64;
		wait10Msec(200);
		motor[hook] = 0;
		turnCW(159, 127);
		wait10Msec(1);
		move(1.3,127);
		wait10Msec(1);
		turnCCW(161, 127);
		wait10Msec(1);
		move(1.1,127);
		wait10Msec(1);
		move(.7,-127);
		wait10Msec(1);
		turnCW(125,127);
		wait10Msec(1);
		move(1.1,127);
		wait10Msec(1);
		turnCCW(125,127);
		wait10Msec(1);
		move(1.4,127);
		wait10Msec(1);
		move(.7,-127);
		wait10Msec(1);
		turnCCW(45,127);
		wait10Msec(1);
		move(1.5,127);
		wait10Msec(1);
	}
	else{
		motor[hook] = -127;
		wait10Msec(60);
		motor[hook] = 64;
		wait10Msec(200);
		motor[hook] = 0;
		turnCCW(159, 127);
		wait10Msec(1);
		move(1.3,127);
		wait10Msec(1);
		turnCW(161, 127);
		wait10Msec(1);
		move(1.1,127);
		wait10Msec(1);
		move(.7,-127);
		wait10Msec(1);
		turnCCW(125,127);
		wait10Msec(1);
		move(1.1,127);
		wait10Msec(1);
		turnCW(125,127);
		wait10Msec(1);
		move(1.4,127);
		wait10Msec(1);
		move(.7,-127);
		wait10Msec(1);
		turnCW(45,127);
		wait10Msec(1);
		move(1.5,127);
		wait10Msec(1);
	}
}
Exemplo n.º 26
0
task main()
{
	waitForStart();
	initializeRobot();
	initialize_gyro();
	StartTask(process_gyro);
	motor[finger] = 40;

	int irSensorVal;
	int colorVal;
	int colorVal2;
	int eopdValleft;
	int LightSensor1, LightSensor2;

	// move forward, turn 45 degrees, move forward
	move(1, 24, 2000);
	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);
		//start new code
		move(1, 9, 5000);
		wait10Msec(100);
		turngyro_left(90.0, 35);
		wait10Msec(100);
		move(1, 12, 5000);
		wait10Msec(100);
		while(LightSensor1 != true && LightSensor2 != true)
		{
			turngyro_left(-40, 15);
			wait10Msec(500);
			turngyro_left(20, 15);
			wait10Msec(500);
			move(1, 2, 5000);
			wait10Msec(500);
			turngyro_left(60, 15);
			wait10Msec(500);
			turngyro_left(-20, 15);
			wait10Msec(500);
			move(-1, 2, 5000);
			wait10Msec(500);
		}
		move(1, 8, 3000);
		wait10Msec(500);
		//end new code

		move(1, 45, 4000);
		wait10Msec(100);
		turngyro_left(83.0, 30);
		wait10Msec(100);
		//move(1, 0.5, 2000);
		nMotorEncoder[arm] = 0;
		wait1Msec(50);

		while(nMotorEncoder[arm] < 1600)
		{
			motor[arm] = 50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = -40;
		wait1Msec(750);
		motor[finger] = 0;
		while(nMotorEncoder[arm] > 800)
		{
			motor[arm] = -50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = 0;
		break;

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

	case 2:
		wait10Msec(100);
		move(-1, 2, 1000);
		turngyro_left(-50.0, 50);
		wait10Msec(50);
		ClearTimer(T1);
		nMotorEncoder[RightMotor] = 0;
		nMotorEncoder[LeftMotor] = 0;
		move(1, 56, 4000);
		ClearTimer(T1);
		move(-1, 12.5, 2000);
		nMotorEncoder[arm] = 0;
		wait1Msec(50);

		while(nMotorEncoder[arm] < 1600)
		{
			motor[arm] = 50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = -40;
		wait1Msec(750);
		motor[finger] = 0;
		while(nMotorEncoder[arm] > 800)
		{
			motor[arm] = -50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = 0;
		break;

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

	case 3:
		move(1, 43, 2000);
		wait10Msec(100);
		turngyro_left(-100.0, 50);
		wait10Msec(100);
		move(1, 2.5, 1000);
		nMotorEncoder[arm] = 0;
		wait1Msec(50);
		while(nMotorEncoder[arm] < 1500)
		{
			motor[arm] = 50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = -40;
		wait1Msec(750);
		motor[finger] = 0;
		while(nMotorEncoder[arm] > 800)
		{
			motor[arm] = -50;
		}
		motor[arm] = 0;
		wait1Msec(1500);
		motor[finger] = 0;
		break;
	}
}
Exemplo n.º 27
0
task main()
{
	waitForStart();
	stopMotors();
	wait10Msec(500);
	moveForward(0.25, 80);
	wait10Msec(50);
	leftTwoWheelTurn(45, 50);
	wait10Msec(56);
	moveForward(41.5, 80);
	wait10Msec(50);
	rightTwoWheelTurn(45, 50);
	wait10Msec(135);
	//robot stopped in the third bucket from the right side of the pendulum
	stopMotors();
	wait10Msec(100);
			motor[tiltingMotor] = 75;
			wait10Msec(98);
			motor[tiltingMotor] = 25;
			wait10Msec(5);
			motor[tiltingMotor] = 0;
			wait10Msec(10);
				motor[conveyorMotor] = 100;
				wait10Msec(200);
				motor[conveyorMotor] = 0;
				wait10Msec(50);
			motor[tiltingMotor] = -35;
			wait10Msec(105);
			motor[tiltingMotor] = -25;
			wait10Msec(5);
			motor[tiltingMotor] = 0;
			wait10Msec(10);
	leftTwoWheelTurn(42, 50);
	wait10Msec(127);
	moveForward(16, 80);
	wait10Msec(50);
	rightTwoWheelTurn(48, 50);
	wait10Msec(53.5);
	moveForward(25, 80);
	wait10Msec(50);
	rightTwoWheelTurn(48, 50);
	wait10Msec(58);
	moveForward(36, 80);
	wait10Msec(50);
	leftTwoWheelTurn(53, 50);
	wait10Msec(115);
	moveBackward(49, 80);
	wait10Msec(50);
	//robot parked in the middle of the ramp
}
task main()
{
    initializeRobot(); //Executes all code in the initializeRobot() function
    StartTask( watchdog ); //Start watchdog task which is a failsafe for the Move function

    waitForStart(); //Wait for the beginning of autonomous phase
    wait10Msec(25); //Pause for 250 miliseconds

    Move(1,40); //Move away from the wall 1 inch at 40% power
    wait10Msec(25); //Pause for 250 miliseconds

    motor[block_arm] = 100; //Give 100% power to the arm motor
    wait10Msec(66); //Allow the arm to run for 660 miliseconds
    motor[block_arm] = 0; //Kill the power to the arm motor
    wait10Msec(25); //Pause for 250 miliseconds

    servo[Claw] = 10; //Move block release servo to open position (10 degrees) allowing it to drop the block
    wait10Msec(100); //Pause for 1000 miliseconds to allow the block to fall

    motor[block_arm] = -100; //Give 100% power to the arm again but in the oppisite direction
    wait10Msec(35); //Allow the arm to move back to a closed position
    motor[block_arm] = 0; //Kill the power to the arm motor
    wait10Msec(25); //Pause for 250 miliseconds

    Turn(-80); //Turn 80 degrees away from the bucket
    wait10Msec(25); //Pause for 250 miliseconds

    Move(22,75); //Move 22 inches towards the ramp at 75% power
    wait10Msec(25); //Pause for 250 miliseconds

    Turn(125); //Turn 55 degrees towards the ramp
    wait10Msec(25); //Pause for 250 miliseconds

    Move(53,75); //Move onto the ramp for the end of the autonomous phase
    wait10Msec(25); //Pause for 250 miliseconds
}
Exemplo n.º 29
0
task main()
{

	writeDebugStreamLine("initalizing");
	initializeRobot();
	writeDebugStreamLine("initalized");
	#ifdef COMPETITION
		waitForStart();
  #else
	while (nNxtButtonPressed != kEnterButton){}
	#endif
	rotateDegrees(45);
	while (nNxtButtonPressed != kRightButton){}
	rotateDegrees(45);
	while (nNxtButtonPressed != kRightButton){}


if (TestHook == 1) {
	setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
	while (nNxtButtonPressed != kRightButton){}
	hookGrab();
	wait1Msec(3000);
	moveInches(-20);
	hookUngrab();
	while (nNxtButtonPressed != kLeftButton){}
}
if (TestHardware == 1) {
		writeDebugStreamLine("Test Hardware");
	trapDoorOpen();
	trapDoorClose();
		servo[collector] = COLLECTOR_IDLE;
 	setPosition (tas, POS_BALL_COLLECTING, DEFAULT_DISTANCE);
wait10Msec(300);
		servo[collector] = COLLECTOR_IN;
wait10Msec(300);
		servo[collector] = COLLECTOR_IDLE;
	setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
  wait10Msec(1000);
	trapDoorOpen();
 wait10Msec(3000);
}
if (TestPositions == 1) {
		writeDebugStreamLine("Test Positions");
	//	while (nNxtButtonPressed != kRightButton){}
	//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
	//	while (nNxtButtonPressed != kLeftButton){}
 //	setPosition (tas, POS_AT_60CM, DEFAULT_DISTANCE);
	//	while (nNxtButtonPressed != kRightButton){}

		//Test 90
	//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
	//	while (nNxtButtonPressed != kLeftButton){}
 //	setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
	//	while (nNxtButtonPressed != kRightButton){}
	//		trapDoorOpen();
	//	while (nNxtButtonPressed != kEnterButton){}

			setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kLeftButton){}
 			setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
		while(nNxtButtonPressed != kEnterButton){}
			moveInches(24);
		while (nNxtButtonPressed != kRightButton){}
			trapDoorOpen();
		while(nNxtButtonPressed != kEnterButton){}
			moveInches(-24);
		while (nNxtButtonPressed != kLeftButton){}
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);

		while (nNxtButtonPressed != kEnterButton){}
 	setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kRightButton){}
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kLeftButton){}
	setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE+3.5);
		while (nNxtButtonPressed != kRightButton){}
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kLeftButton){}
	setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kRightButton){}
	trapDoorOpen();
		while (nNxtButtonPressed != kRightButton){}
	setPosition (tas, POS_AT_30CM, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kLeftButton){}
	//setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		//while (nNxtButtonPressed != kRightButton){}
	setPosition (tas, POS_AT_90CM, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kEnterButton){}
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kRightButton){}
	setPosition (tas, POS_AT_120CM, DEFAULT_DISTANCE);
		//while (nNxtButtonPressed != kLeftButton){}
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
		while (nNxtButtonPressed != kEnterButton){}

}
#if 0
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
	wait10Msec(200); //wait 2 sec for raise
	//moveInches(50);
	moveInches(54);
	setPosition(tas, POS_AT_60CM, DEFAULT_DISTANCE-1.5);
  wait10Msec(1000);
	//moveInches(14);
	moveInches(10);
	trapDoorOpen();
	wait10Msec(500);
	moveInches(-20);
	setPosition (tas, POS_DRIVE, DEFAULT_DISTANCE);
	wait10Msec(2000);
#endif
}
Exemplo n.º 30
0
task main()
{
	initializeRobot();

	unsigned short waitValue;
	while (nNxtButtonPressed != 0 && nNxtButtonPressed != 3){
  	nxtDisplayCenteredTextLine(0, "%d milliseconds", waitValue);
  	if (nNxtButtonPressed == 1){
  		waitValue += 1000;
  		wait1Msec(325);
  	}
  	if (nNxtButtonPressed == 2){
  		waitValue -= 1000;
 			wait1Msec(300);
 		}
  	if (waitValue > 18000)
  		waitValue = 0;
  	if (waitValue < 0)
  		waitValue = 18000;
  }

  nxtDisplayCenteredTextLine(0, "Wait Time Done");

  waitForStart();

  wait1Msec(waitValue);

  ClearTimer(T1);
  ClearTimer(T2);

  resetEncoders();
  while (time1[T2] < firstMove){
  	move(-80);
  	if (time1[T1] < 1750){
  		motor[Lift1] = 100;
      motor[Lift2] = 100;
    }
    else
    {
    	motor[Lift1] = 0;
      motor[Lift2] = 0;
    }
  }
  move(0);
  while (time1[T1] < 1750){
  	motor[Lift1] = 100;
  	motor[Lift2] = 100;
  }
  motor[Lift1] = 0;
  motor[Lift2] = 0;

  //dump cube
  servo[bucket] = 170;
  wait1Msec(2000);
  servo[bucket] = 135;

  //continue to lift
  resetEncoders();
  ClearTimer(T1);
  ClearTimer(T2);
  while (time1[T2] < secondMove){
  	move(-80);
  	if (time1[T1] < 1300){
  		motor[Lift1] = -100;
      motor[Lift2] = -100;
    }
    else
    {
    	motor[Lift1] = 0;
      motor[Lift2] = 0;
    }
  }
  move(0);



  wait10Msec(50);
  resetEncoders();
  ClearTimer(T2);
  while (time1[T2] < turnTime){
  	turn(-80);
 		if (time1[T1] < 1300){
  		motor[Lift1] = -100;
      motor[Lift2] = -100;
    }
    else
    {
    	motor[Lift1] = 0;
      motor[Lift2] = 0;
    }
  }
  move(0);

  resetEncoders();
  while (time1[T2] < upRamp){
  	move(-80);
  	if (time1[T1] < 1300){
  		motor[Lift1] = -100;
      motor[Lift2] = -100;
    }
    else
    {
    	motor[Lift1] = 0;
      motor[Lift2] = 0;
    }
  }

  move(0);

   while (time1[T1] < 1300){
  	motor[Lift1] = -100;
  	motor[Lift2] = -100;
  }
  motor[Lift1] = 0;
  motor[Lift2] = 0;
}