自学教程：C++ wait10Msec函数代码示例

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;}	}}

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://.........这里部分代码省略.........

/////////////////////////////////////////////////////////////////////////////////////////////////////////                                         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);//.........这里部分代码省略.........

 //=============================================================================// 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);//.........这里部分代码省略.........

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;    }  }}

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}

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;//////.........这里部分代码省略.........

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);//.........这里部分代码省略.........

task main(){  accelerate(50);  wait10Msec(100);}

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();//.........这里部分代码省略.........

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);//.........这里部分代码省略.........

void chill(){  motor[rightWheel] = 0;  motor[leftWheel] = 0;  wait10Msec(100);}

void Right90deg(){  motor[rightWheel] = TurnSpeed*-1;  motor[leftWheel] = TurnSpeed;  angle = (angle-90 )% 360;  wait10Msec(TurnTime);}

/////////////////////////////////////////////////////////////////////////////////////////////////////////                                    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;			}//.........这里部分代码省略.........

void deploy(){	intake(-1);	wait10Msec(70);	intake(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;	}}

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)//.........这里部分代码省略.........

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); }  }}

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;  }}

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;	}}

//************************// 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);    //=====================================  }}

//.........这里部分代码省略.........	}	// 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);

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);	}}

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);//.........这里部分代码省略.........

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}

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);//.........这里部分代码省略.........

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;    }//.........这里部分代码省略.........