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

/*-----------------------------------------------------------------------------*/taskiqMenuTask(){    static  int select = 0;            bool update = true;    // This should allow using the exit button but it doesn't seem to work    bUserControlsLCDButtons = true;    // Run menu forever    while( true ) {        if( update ) {            // Update main menu, only three items            iqDisplayLine( LCD_ROW1, select == 0, "Run" );            iqDisplayLine( LCD_ROW2, select == 1, "Configure" );            iqDisplayLine( LCD_ROW3, select == 2, "Reset to Default" );        update = false;        }        // Check LCD buttons        if( nLCDButtons != kButtonNone ) {            if( nLCDButtons == kButtonDown  )                if( select++ == 2 ) select = 2;            if( nLCDButtons == kButtonUp  )                if( select-- == 0 ) select = 0;            if( nLCDButtons == kButtonSelect  ) {                // reset                if( select == 2 ) iqResetToDefaults();                // Enter configure menu                if( select == 1 ) iqMenuConfiguration();                // Enter run mode                if( select == 0 ) {                    // Set everything as enabled                    robotRunning = true;                    robotEnabled = true;                    // Start the control task                    startTask( iqRunRobotTask, kDefaultTaskPriority + 1 );                    // and display information on the LCD while running...                    iqMenuRun();                    // When we return to this point user has indicated program should stop                    // force task to stop motors and quit                    robotRunning = false;                    wait1Msec(100);                    // task should be done                }            }        // Wait for LCD button release        while( nLCDButtons != kButtonNone )          wait1Msec(10);        // update display        update = true;        }    // Don't hog cpu    wait1Msec(25);    }}

int sucker(int speed, int duration) { //positive numbers for out  setSuckSpeed(speed);  wait1Msec(duration);  setSuckSpeed(0);  return 1;}

void PIDDriver(short target){  float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;  short u;  time1[T1] = 0;  time1[T2] = 0;  time1[T4] = 0;  float pre_compass = compass();  bool greyPatch = false;  while(true){    if(time1[T1] < timeout){ // T1 for timeout        e = threshold - SensorValue[Light];        e_dot = e - old_e;        E += e;        u = (short)(KP * e + KD * e_dot + KI * E);        old_e = e;        motor[Left] = cruise_speed - u;        motor[Right] = cruise_speed + u;        if(SensorValue[Light] < target){ //target can be make more conservative than threshold          time1[T1] = 0;        }        // When grey patch detected        if(SensorValue[Light] > 38 && SensorValue[Light] < 43){            if(time1[T4] > 300){                greyPatch = true;                stop();                // beep three times                PlayTone(1500, 40);                wait1Msec(800);                PlayTone(1500, 40);                wait1Msec(800);                PlayTone(1500, 40);                wait1Msec(800);                control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch                while(SensorValue[Light] > threshold){}                stop();                time1[T1] = 0;                time1[T2] = 0;                time1[T3] = 0;                nMotorEncoder[Left]=0;                nMotorEncoder[Right]=0;                pre_compass = compass();                continue;            }        }        else{            time1[T4] = 0;        }        if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps            float cur_compass = compass();            if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){                PlayTone(1175, 20);                time1[T3] = 0; // T3 for beepInterval            }            pre_compass = cur_compass;            time1[T2] = 0; // T2 for sampling frequency            if(greyPatch){ // log the compass value from grey patch to the other endpoint              float tmp = signedCompass();              nxtDisplayStringAt(0,31,"%f", tmp);              logCompass(tmp);            }        }    }    else{        if(greyPatch){          if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return        }        else{          turnOnSpot(target, greyPatch);        }    }  }}

task main (){  ubyte counter = 0;  ubyte oldCounter = 0;  ubyte refSignal = 0;  ubyte oldRefSignal = 0;  ubyte txType = 0;  long rawValue = 0;  nxtDisplayCenteredTextLine(0, "Mindsensors");  nxtDisplayCenteredBigTextLine(1, "PSP-Nx");  nxtDisplayCenteredTextLine(3, "Test 2");  wait1Msec(2000);  PlaySound(soundBeepBeep);  while(bSoundActive) EndTimeSlice();  eraseDisplay();  while (true)  {    // Get the current counter value, wraps at 255    counter = PSPV4readRefSignalCount(PSPNXV4);    if (oldCounter != counter)    {      // This is the command sent by the remote control, can be:      // PSPV4_SIGNAL_FAST_REWIND      // PSPV4_SIGNAL_FAST_FORWARD      // PSPV4_SIGNAL_PLAY      // PSPV4_SIGNAL_STOP      refSignal =  PSPV4readRefSignal(PSPNXV4);      // Get the transmitted type.  Not very useful in most cases unless      // you need to know what type of transmitter sent the command			txType = PSPV4readRefTXType(PSPNXV4);			// Raw value will also "see" commands from the remote			// that are not recognised as "play", "stop", "forward" or "rewind".			// You could use this to control additional aspects of your robot!			rawValue = PSPV4readRawRefTXValue(PSPNXV4);      if (oldRefSignal != refSignal)      {	      PlaySound(soundShortBlip);		    switch(refSignal)		    {		      case PSPV4_SIGNAL_FAST_REWIND:   nxtDisplayCenteredBigTextLine(3,"REWIND"); break;		      case PSPV4_SIGNAL_FAST_FORWARD:  nxtDisplayCenteredBigTextLine(3,"FORWARD"); break;		      case PSPV4_SIGNAL_PLAY:          nxtDisplayCenteredBigTextLine(3,"PLAY"); break;		      case PSPV4_SIGNAL_STOP:          nxtDisplayCenteredBigTextLine(3,"STOP"); break;		    }		  }		  // Always display the raw value, even if it's not one of the four		  // referee signals.  Handy if you want to add more commands to your robot!	    nxtDisplayTextLine(6, "Type:  0x%02X", txType);	    nxtDisplayTextLine(7, "Raw:  0x%03X", rawValue);	    // Update the counters and signals	    oldCounter = counter;	    oldRefSignal = refSignal;	  }    wait1Msec(50);  }}

void Ultra_Seek( int Min, int Max ){	int state = 2;  int time;  int time_old = 0;  int delta_time;  float maxVelocity = 180.0; // max angular velocity in deg/sec  float GyroOld;  float GyroNew;  int rotDirection = 1;  int motorSpeed = 0;  float DegreeGain = 5.0;  float degreesToGo;  int MAX_GYRO_SPEED  = 85; //maximum motor speed allowed in turns  int MIN_GYRO_SPEED  = 40; //minimum motor speed allowed in turns  float Angle;  Angle = 0.0;	degreesToGo = 0.0;	time_old = nPgmTime;	GyroOld = 0.0;	wait1Msec(100);	while(SensorValue[SONAR_1] > Min)		{			while (SensorValue[SONAR_1] > Min && SensorValue[SONAR_1] < Max)			{				motor[rightDrive] = 20;				motor[leftDrive] = 20;			}				motor[rightDrive] = 0;				motor[leftDrive] = 0;				wait10Msec(55);				time100[T1] = 0;				time100[T2] = 0;			while(SensorValue[SONAR_1] > Max)			{				if(time100[T1] < 30)				{				 motor[rightDrive] = 20;				 motor[leftDrive] = -20;		  	}		  	if(time100[T2] > 30)				{				 motor[rightDrive] = -20;				 motor[leftDrive] = 20;		  	}			}			ClearTimer(T3);			time =  nPgmTime; // this timer wraps around    delta_time = abs(time - time_old); // delta time in ms    if (delta_time < 1) // protect against divide by zero    {      delta_time = 1;    }    // read the gyro sensor minus the bias offset. GyroBias must be declared and    // computed in the calling program.    GyroNew = -((float)SensorValue[Gyro] - GyroBias);    // limit the gyro to the max achievable by the bot to minimize data spikes.    if (abs(GyroNew) > maxVelocity) GyroNew = sgn(GyroNew)*maxVelocity;    // deadband for the gyro to eliminate drift due to noise    if (abs(GyroNew) <= 0.2) GyroNew = 0.0;    // compute the integral of the angular rate using a trapazoidal approximation    // http://en.wikipedia.org/wiki/Numerical_integration    Angle = Angle + 0.001 * (float)delta_time * 0.5 *(GyroNew + GyroOld);    // update the old values for the next time through the loop    time_old = time;    GyroOld = GyroNew;		}} // End of Move()

void deploy(){	intake(true);	wait1Msec(300);	stopIntake();}

/**Time-Based Intake**/void intake(int power, int time){	motor[intakeMotor] = power;	wait1Msec(time);		//Time intake runs	motor[intakeMotor] = 0;}

////// PID drive //////void Drive(int DistanceIN) {	long error = 99999;	float KP = 0.35;	float KD = 2.1;	float KI = 1;	long Velocity = 0;	long Integral = 0;	long CurrentDistance;	long PreviousDistance = 0;	SensorValue(LeftEncoder) = 0;	SensorValue(RightEncoder) = 0;	SensorValue(TurnGyro) = 0;	int Distance;	int SpeedCorrect = 0;	int MotorSpeed = 0;	int cycle = 0;	Distance = DistanceIN / 0.024;	while (abs(error) > 5) {		CurrentDistance = ((SensorValue(RightEncoder)-SensorValue(LeftEncoder))/2);		if (Velocity < 10){			if(error > 0){				Integral++;			}			else {				Integral = 0;			}		}		else {			Integral = 0;		}		// P = calculate the error		error = Distance - CurrentDistance;		// D = Calculate the speed		Velocity = CurrentDistance - PreviousDistance;		if (SensorValue(TurnGyro) < -2){			// Edit how much is added to change how fast it reacts			SpeedCorrect = SpeedCorrect - 0.7;		}		else if (SensorValue(TurnGyro) > 2){			SpeedCorrect = SpeedCorrect + 0.7;		}		// Calculates how fast to move the robot		MotorSpeed = (error * KP) + (Integral * KI) - (Velocity * KD);		BaseSpeed(MotorSpeed - SpeedCorrect, MotorSpeed + SpeedCorrect);		writeDebugStreamLine("Cycle: %d", cycle);		// Save the distance for the next cycle		PreviousDistance = CurrentDistance;		wait1Msec(25);	}	BaseSpeed(0,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;//.........这里部分代码省略.........

task main(){	motor[Motor] = 100;	wait1Msec(1000);	motor[Motor] = 0;}

void moveIntakeTop(int time, int power) {	motor[intakeTop] = power;	wait1Msec(time);	motor[intakeTop] = 0;}

task main(){  initializeRobot();	int countline = 0;  waitForStart(); // Wait for the beginning of autonomous phase.  motor[slide] = 20;	wait1Msec(750);	motor[slide] = 0;	ClearTimer(T1);	SensorValue[irSeek] = 0;	motor[leftDrive]  = 70;	motor[rightDrive] = 70;	servo[leftgrab] = grabDownPosition;  while(time1[T1] < 9000)//stays in loop while less than 9 seconds	{		if(atLine())		{			countline++; // if at line check and see if we are at the IR beacon if break out of loop and go to next loop			if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6)			{				// at Correct Line				PlayTone(100,100);				break;			}			else if(countline==1)			{				motor[rightDrive]= 5;//if it isnt at the beacon and its the first line turn to the right slightly				motor[leftDrive] = 0;				wait1Msec(500);			}			else if(countline == 2)/*may need to change*/{				motor[rightDrive] = 4;//if it isn't at the beacon and is at line 2				motor[leftDrive] = 0;				wait1Msec(500);			}			motor[leftDrive]  = 75;// drive with 75% power			motor[rightDrive] = 75;			wait1Msec(500);		}	}	if(time1[T1] < 9000)//while less than 9 seconds and boolean atgood is true	{		nMotorEncoder[leftDrive]  = 0;		nMotorEncoder[rightDrive] = 0;		if(countline == 1)		{//if it is at the beacon and it is at the first line			forward(6); //Distance after line till stop			stopDrive();			wait1Msec(500);			servo[leftgrab] = grabReleasePositionpeg1;//put grabber at position			wait1Msec(500);			motor[slide] = -50;			wait1Msec(1000);			motor[slide] = 0;			wait1Msec(1000);	    servo[leftgrab] = grabReleasePositionpeg1 - 15;// move slightly to possible help	    wait1Msec(1000);	    motor[leftDrive] = -30;	    motor[rightDrive] = -30;	    wait1Msec(1000);			motor[leftDrive] = 0;			motor[rightDrive] = 0;		}		else if(countline == 2)//		{//if at line 2 and at the beacon			forward(7);//Distance after line till stop			stopDrive();			wait1Msec(500);			servo[leftgrab] = grabReleasePositionpeg2;//move to position for the 2nd peg			wait1Msec(500);			motor[slide] = -50;//slide over to put the ring on			wait1Msec(1000);			motor[slide] = 0;			wait1Msec(1000);		    servo[leftgrab] = grabReleasePositionpeg2 + 10;//move slightly to help put the ring on		    wait1Msec(1000);		    motor[leftDrive] = -50;		    motor[rightDrive] = -50;		    wait1Msec(2000);			motor[leftDrive] = 0;			motor[rightDrive] = 0;		}		else if(countline ==3)		{//if at line 3 and at beacon//.........这里部分代码省略.........

void StopMotors(){	driveMotors(0,0);	wait1Msec(20);}

//--------------------------------------------------------------------------------------------------------------task main(){																																// Start Main Loop	initializeRobot(); 																						// Get ready! see void initalizerobot() above	waitForStart();   																						// wait for start of tele-op phase From FCS	// Start the loop ----------------------------------------------------------------------------------------------	while(true)	{																															// Start While loop		// Right and Left Joysticks-------------------------------------------------------------------------------------		getJoystickSettings(joystick);  															// Update Buttons and Joysticks		motor[motorLEFT]  = scaleForMotor(joystick.joy2_y1);		motor[motorRIGHT] =-scaleForMotor(joystick.joy2_y2);		// Buttons------------------------------------------------------------------------------------------------------		if(joy1Btn (1)== 1)  		    																// If Joy1-Button (1 blue X) is pressed:		{																													//1open bracket			ENCODER_1_TRGT = ARM_1_COUNT_1 - nMotorEncoder[motorARM1];			ENCODER_2_TRGT = ARM_2_COUNT_1 - nMotorEncoder[motorARM2];			ARM_2_STATE = 1;																			// Trigger for Stage 2			//---------			if (ENCODER_1_TRGT > DEAD_ARM_1)			{																											//2open bracket				PlaySound(soundShortBlip);				wait1Msec(500);				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM				motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:				{																										//3open - wait for motor to catch up					servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position					servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves					motor[motorRIGHT] = -scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves				}																										//3close				ENCODER_1_TRGT = 0;				motor[motorARM1] = 0;              									// stop motor and hold position			}			if (ENCODER_2_TRGT > DEAD_ARM_2)			{				nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM				motor[motorARM2] = POWER_2;               						// motor ARM is run at high power level				while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:				{																										// wait for motor to catch up					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves				}				servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position				servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position				ENCODER_2_TRGT = 0;			}																											// 2close			//---------			if (ENCODER_2_TRGT <  -DEAD_ARM_2)			{				nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM				motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level				while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:				{																										// wait for motor to catch up					//						servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position					//						servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves				}				servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position				servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position				ENCODER_2_TRGT = 0;			}			else if (ENCODER_1_TRGT <  -DEAD_ARM_1)			{																											//4open				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:				{																										//5open - wait for motor to catch up					servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position					servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves				}																										//5close				motor[motorARM1] = 0;              									// stop motor and hold position				ENCODER_1_TRGT = 0;			}																											//4close			//---------			motor[motorARM1] = 0;              									// stop motor and hold position			motor[motorARM2] = 0;              									// stop motor and hold position		} 																														//1close		// Button 2 Stage 1----------------------------------------------------------------------------------------------		else if(joy1Btn (2)== 1)																				// If Joy1-Button (2 green B) is pressed:		{																													//1open bracket			ENCODER_1_TRGT = ARM_1_COUNT_2 - nMotorEncoder[motorARM1];			ENCODER_2_TRGT = ARM_2_COUNT_2 - nMotorEncoder[motorARM2];			//---------			if (ENCODER_1_TRGT > DEAD_ARM_1)			{while (ENCODER_2_TRGT > DEAD_ARM_2)				{					nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM					motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level					while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:					{																										// wait for motor to catch up						servo[clawservo1] = SERVO_ANGLE_2;									// move servo to new position						servo[clawservo2] = SERVO_ANGLE_2;									// move servo to new position//.........这里部分代码省略.........

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;	  //********    case MT_LL:	    LL_Avr = LLreadAverage(PORT_LL);	    LL_IO  = LLreadResult(PORT_LL);	    break;	  //********    case MT_US:	    wait1Msec(30);	    ReadAllUS_short(PORT_ARD,USwall);	    break;  }}

task main(){	init();	waitForStart();	driveSonar(1,25,80);	allStop();	wait1Msec(100);	backward(20);	wait1Msec(700);	sticksDown();	wait1Msec(250);	allStop();	wait1Msec(100);  	raiseLift(100);  	time1[T1]=0;  	while (nMotorEncoder[intake] < 410 && time1[T1] < 2500) //while the encoder wheel turns one revolution  	{  		times = time1[T1];  	}	allStop();	wait1Msec(500);	//if(time1[T1]>2500)	//	while(true){	//		nxtDisplayCenteredBigTextLine(1,":(");	//	}	releaseBalls();	allStop();	wait1Msec(3000);	retainBalls();	allStop();	wait1Msec(750);  	lowerLift(80);  	while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution  	{  	}	allStop();	wait1Msec(500);	releaseAutoBall();	wait1Msec(500);	motor[intake] = 100;	wait1Msec(1000);	motor[intake] = 0;	turn(0,180,100);	allStop();	wait1Msec(100);	drive(1,1,75);	sticksUp();	drive(0,1,75);	time1[T1]=0;	while(SensorValue[sonarSensor]>30||time1[T1]<1200){		left(50);	}	while(SensorValue[sonarSensor]<200){		left(50);	}	allStop();	wait1Msec(100);	turn(0,25,70);	allStop();	wait1Msec(100);	driveSonar(1,25,50);	backward(30);	wait1Msec(700);	sticksDown();	wait1Msec(250);	allStop();	raiseLift(100);  	while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution  	{  	}	allStop();	wait1Msec(500);	releaseBalls();	allStop();	wait1Msec(3000);	retainBalls();	allStop();	wait1Msec(750);  	lowerLift(80);  	while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution  	{  	}	allStop();	wait1Msec(500);}

task main() {	RobotState state;	initialize(&state);	short leftSpeed, rightSpeed;	short prevState = INITIALSTATE;	bool distLess50;	bool irDetected = false;	bool sawRedBlue = false;	bool goBackward = false;	float startAngle = 0;	//waitForStart();	wait1Msec(state.delayTime*1000);	INITIALDRIVE();	while(true){		//if state changes: stop motors, play tone, reset timers, gyro and lights		if (prevState != state.currentState){			if (prevState != PARK_DRIVE2 && prevState != CHECKEND){				stopMotors();				leftSpeed = 0;				rightSpeed = 0;			}			AUDIO_DEBUG(500, 10);			time1[T1] = 0;			resetGyroAccel(&state);			LEDController(0x00);			distLess50 = false;			startAngle = state.degrees;		}		getSensors(&state);		prevState = state.currentState;		if(state.currentState == FINDLINE_TURN){			drive(0, TURNSPEED);			if(state.color2 == RED || state.color2 == BLUE){				sawRedBlue = true;			}			if (sawRedBlue && state.color2 == BLACK){				state.currentState = LINEFOLLOW;			}			if(abs(state.degrees) > 10){				state.currentState = LINEFOLLOW;			}		/* state FINDLINE_DRIVE seems unnecessary		} else if (state.currentState == FINDLINE_DRIVE) {			drive(DRIVESPEED, DRIVESPEED*.95);			if (state.irDir == 5 && irDetected == false) {				state.currentState = SCOREBLOCK;			} else if(state.color2 == RED || state.color2 == BLUE){				state.currentState = LINEFOLLOW;			} */		} else if (state.currentState == LINEFOLLOW) {			if (state.dist < 50) {				distLess50 = true;			}			if (state.irDir == 5 && irDetected == false) {				state.currentState = SCOREBLOCK;			} else if (state.dist > 50 && distLess50 && irDetected == true) {				state.currentState = GOTOEND;			} else if (goBackward){				if (state.color == RED || state.color == BLUE) {					leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;					rightSpeed = -DRIVESPEED;				}else {					leftSpeed = -DRIVESPEED;					rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;				}			} else {				if (state.color2 == RED || state.color2 == BLUE) {					leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;					rightSpeed = DRIVESPEED;				} else {					leftSpeed = DRIVESPEED;					rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;				}			}			drive(leftSpeed, rightSpeed);		} else if (state.currentState == SCOREBLOCK) {			irDetected = true;			goBackward = true;			LEDController(B2);			blockScorer();			state.currentState = LINEFOLLOW;		} else if (state.currentState == GOTOEND) {			drive(-DRIVESPEED, -DRIVESPEED);			if(time1[T1] >= 300) {				state.currentState = PARK_TURN1;			}		} else if (state.currentState == PARK_TURN1) {			drive(-TURNSPEED, TURNSPEED);			if(abs(state.degrees) >= 15){				state.currentState = PARK_DRIVE1;			}		} else if (state.currentState == PARK_DRIVE1) {			drive(-DRIVESPEED, -DRIVESPEED);			if(state.color == RED || state.color == BLUE){				state.currentState = PARK_TURN2;			}		} else if (state.currentState == PARK_TURN2) {//.........这里部分代码省略.........

///////////////////////////////////////////////////////////////////////////////////////////////////////                                         Main Task// The following is the main code for the autonomous robot operation.// Sequence of Operations:// 1. Detect the IR beam// 2. Drop the block// 3. Backup, Turn, Go Back, Turn// 4. Climb the Ramp// At the end of the autonomous period, the FMS will autonmatically abort execution of the program./////////////////////////////////////////////////////////////////////////////////////////////////////task main(){	//initialize the servos  initializeRobot();  bFloatDuringInactiveMotorPWM = true;	// Wait for the beginning of autonomous phase.  waitForStart();  servo[servoIRDrop]= 240;	//raise the rack so that the robot can move easily  RaiseRack();  //move the arm up so that the robot can move easily  motor[motorArm] = 65;  wait1Msec(1000);  motor[motorArm] = 0; 	//detect the IR beam, we get out when 5 is detected 	//returns the number of ticks moved 	int currentTicks = MoveUntilIR(); 	//move up a bit 	Move(6, FORWARD, 35);	wait1Msec(30);	//drop the block 	DropBlockIR();  //Bring the drop block mechanism higher, 	//so that its not in the way of the center part of the beam  servo[servoIRDrop] = 255 ;	wait1Msec(70);	//backup  //dont go to the start position, start a bit before, so that wall will not be touched  nMotorEncoder[motorLeft] = 0;  nMotorEncoderTarget[motorLeft] =  (currentTicks * -1)  + 550;  //power  the right motor earlier than the left motor  motor[motorRight] = -55;  wait1Msec(50);  //power the left motor  motor[motorLeft] = -55;  //wait for it go back  while ( nMotorRunState[motorLeft] != runStateIdle )  {  }  //stop the motors  motor[motorLeft] = 0;  motor[motorRight] = 0;	wait1Msec(50);	//turnright towards the ramp	PointTurnRight(1500);	wait1Msec(30);	//go back a bit for more clearance	Move(50, BACKWARD, 55);	wait1Msec(30);	PointTurnRight(1900);	wait1Msec(30);	Move(40, BACKWARD, 55);	wait1Msec(30);	//brake instead of coast	bFloatDuringInactiveMotorPWM = false;	//check for other robots	if(SensorValue[sonarSensor] <= 75) //if obstruction is present, max power		Move(18, BACKWARD, 90);	else if(SensorValue[sonarSensor] > 75)		Move(18, BACKWARD, 60);	//bring it down so it is not in the way of the rack motor	servo[servoIRDrop] = 200;	wait1Msec(70);	// We are done  while (true)	{//.........这里部分代码省略.........

/**Time-Based Tread**/void tread(int power, int time){	motor[treadMotor] = power;	wait1Msec(time);		//Time intake runs	motor[treadMotor] = 0;}

void DropBlockIR(){	servo[servoIRDrop]=75;	wait1Msec(700);}

// Put the main driver control loop in its own tasks so// the drivers never loses control of the robot!task drive(){    // Initialize variables    int lastMessage = 0;    int leftMotorSpeed = 0;    int rightMotorSpeed = 0;    int armMotorSpeed = 0;    //int specMotorSpeed = 0;    int totalMessages = 0;    int topSpeed = MOTOR_POWER_DOWN_MAX;    int armTopSpeed = 65;    //int specTopSpeed = 20;    int scoopTopSpeed = 40;    int scoopMotorSpeed = 0;    while (true)    {        getJoystickSettings(joystick);        if (lastMessage != ntotalMessageCount) {            if (true) {                ClearTimer(T2);                lastMessage = ntotalMessageCount;                // New joystick messages have been received!                // Set your drive motors based on user input                // here.                leftMotorSpeed = joystick.joy1_y1 / JOYSTICK_Y1_MAX * topSpeed; // Map the leftMotorSpeed variable to joystick 1_y1                if (abs(leftMotorSpeed) < JOYSTICK_DEAD_ZONE) leftMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone                rightMotorSpeed = joystick.joy1_y2 / JOYSTICK_Y1_MAX * topSpeed; // Map the rightMotorSpeed variable to joystick 1_y2                if (abs(rightMotorSpeed) < JOYSTICK_DEAD_ZONE) rightMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone                armMotorSpeed = joystick.joy2_y2  / JOYSTICK_Y1_MAX * armTopSpeed; // Map the armMotorSpeed variable to joystick 2_y2                if (abs(armMotorSpeed) < JOYSTICK_DEAD_ZONE) armMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone                scoopMotorSpeed = joystick.joy2_y1  / JOYSTICK_Y1_MAX * scoopTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1                if (abs(scoopMotorSpeed) < JOYSTICK_DEAD_ZONE) scoopMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone                //specMotorSpeed = joystick.joy1_x1  / JOYSTICK_Y1_MAX * specTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1                //if (abs(specMotorSpeed) < JOYSTICK_DEAD_ZONE) specMotorSpeed = 0;                motor[armMotor] = armMotorSpeed; // Set the motor armMotor speed as armMotorSpeed                motor[leftMotor] = leftMotorSpeed; // Set the motor leftMotor speed as leftMotorSpeed                motor[rightMotor] = rightMotorSpeed; // Set the motor rightMotor speed as rightMotorSpeed                motor[scoopMotor] = scoopMotorSpeed;                //motor[specMotor] = specMotorSpeed;                if (joy1Btn(5) == 1) {                    // Power up                    topSpeed = MOTOR_POWER_UP_MAX;                }                if (joy1Btn(7) == 1) {                    // Power down                    topSpeed = MOTOR_POWER_DOWN_MAX;                }                if (joy2Btn(2) == 1) {                    // Power up                    armTopSpeed = ARM_MOTOR_POWER_DOWN;                }                if (joy1Btn(4) == 1) {                    // Power up                    armTopSpeed = ARM_MOTOR_POWER_UP;                }                if (joy1Btn(4) == 1) {                    // Nudge forward                    motor[leftMotor] = NUDGE_POWER;                    motor[rightMotor] = NUDGE_POWER;                    wait1Msec(NUDGE_DURATION);                    motor[leftMotor] = 0;                    motor[rightMotor] = 0;                    wait1Msec(NUDGE_DELAY);                }                if (joy1Btn(2) == 1) {                    // Nudge backward                    motor[leftMotor] = NUDGE_POWER * -1;                    motor[rightMotor] = NUDGE_POWER * -1;                    wait1Msec(NUDGE_DURATION);                    motor[leftMotor] = 0;                    motor[rightMotor] = 0;                    wait1Msec(NUDGE_DELAY);                }                if (joy1Btn(0) == 1) {                    // Nudge left                    motor[leftMotor] = 0;                    motor[rightMotor] = NUDGE_POWER;                    wait1Msec(NUDGE_DURATION);                    motor[leftMotor] = 0;                    motor[rightMotor] = 0;//.........这里部分代码省略.........

task main(){	// Gekregen code voor BlueTooth	string sig = "";	TFileIOResult nBTCmdRdErrorStatus;	int nSizeOfMessage;	ubyte nRcvBuffer[kMaxSizeOfMessage];	// Einde gekregen code voor BlueTooth  while(true) // Main loop  {  	/*		 * Gekregen code voor BlueTooth		 * leest berichten die BlueTooth stuurt uit en stopt die in een string.		 */    nSizeOfMessage = cCmdMessageGetSize(INBOX);    if (nSizeOfMessage > kMaxSizeOfMessage)      nSizeOfMessage = kMaxSizeOfMessage;    if (nSizeOfMessage > 0){    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);    	nRcvBuffer[nSizeOfMessage] = '/0';    	string s = "";    	stringFromChars(s, (char *) nRcvBuffer);    	displayCenteredBigTextLine(4, s);    /*		 * Einde gekregen code voor BlueTooth		 */    	sig = nRcvBuffer;// zet bluetoothsignaal in variabele    	//writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven    	// naar voren rijden zonder lijndetectie    	if(sig == "U"){   			setMotor(motorA, 30);   			setMotor(motorB, 30);   			wait1Msec(10);   		}   		// naar achter rijden zonder lijndetectie			if(sig=="D"){				setMotor(motorA, -30);				setMotor(motorB, -30);				wait1Msec(10);			}			// maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten)			if(sig=="L"){				setMotorTarget(motorA, 0, 0);				setMotorTarget(motorB, 360, 30);				waitUntilMotorStop(motorB);				startTask(Rijden);				k=1; // zet anti-lijnhump op 1				l=1; // zet anti-lijnhump op 1			}    	// maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten)  		if(sig=="R"){  		  setMotorTarget(motorA, 360, 30);			  setMotor(motorB, 0);			  waitUntilMotorStop(motorA);			  startTask(Rijden);				k=1; // zet anti-lijnhump op 1				l=1; // zet anti-lijnhump op 1			}			// rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten)			if(sig=="F"){				setMotorTarget(motorA, 180, 30);				setMotorTarget(motorB, 180, 30);				waitUntilMotorStop(motorA);				waitUntilMotorStop(motorB);				k=1; // zet anti-lijnhump op 1				startTask(Rijden);			}			// Stoppen			if(sig=="A"){				setMultipleMotors(motorA, motorB, 0);				stopTask(Rijden);				clearSounds();			}			// Zet lijndetectie aan			if(sig=="C"){				k=1; // zet anti-lijnhump op 1				l=1; // zet anti-lijnhump op 1				startTask(Rijden);			}			/*			 * nog 1 knop ongebruikt (B).			 */			// Na elk commando, wacht 0,1s.    	wait1Msec(100);    }	}}

task main(){	//wait	servo[topServo] = 235;	initializeRobot();	StartTask(getHeading);	wait1Msec(400);  forwardInc(48, 30);  //Calculates the positioning of the center goal after 10 position tics at 200msec intervals	float a = 0;	float b = 0;	float c = 0;  wait1Msec(400);  StartTask(infared);	wait1Msec(200);	int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a;	int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b;	for(float i = 0; i <= 10; i++){      if(ir1 < ir2-10){      	PlayImmediateTone(440, 10);        a += 1;      }else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2      	PlayImmediateTone(1024, 10);      	b += 1;      }else if(ir1 > ir2+10){      	PlayImmediateTone(2024, 10);				c += 1;      }else{      	PlayImmediateTone(1024, 10);      	//b += 1;      }      wait1Msec(200);  }  ClearSounds();  //code for each of the three center positions a=1 b=2 c=3  StopTask(getHeading);  wait10Msec(50);  StartTask(getHeading);	wait1Msec(200);  if(a > (b+c)/2){  	//PlayImmediateTone(440, 10);  	currHeading = 0;		turnDeg(-20, 25);		wait10Msec(50);  	forwardInc(74, 30);  	wait10Msec(50);  	StopTask(getHeading);  	currHeading = 0;  	wait1Msec(200);  	StartTask(getHeading);  	wait1Msec(100);  	turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr  	wait10Msec(25);  	forwardInc(-22, 25);  	liftPos(3);  	servo[topServo] = 210;//original 150  	wait10Msec(25);  	servo[topServo] = 235;  	wait10Msec(500);  	liftPos(0);  	wait10Msec(500);  }else if(b > (a+c)/2){  	PlayImmediateTone(1024, 10);  	StopTask(getHeading);  	currHeading = 0;  	wait1Msec(200);  	StartTask(getHeading);  	wait1Msec(100);		turnDeg(180, 25); //do a one 80		StopTask(getHeading);  	currHeading = 0;  	wait1Msec(200);  	StartTask(getHeading);		wait10Msec(70);  	forwardInc(-36, 30);//go back __ inches  	//StartTask(getHeading);  	//wait1Msec(100);		//turnDeg(20, 25);  	wait10Msec(200);  	liftPos(3);  	servo[topServo] = 210;//original 150  	wait10Msec(25);  	servo[topServo] = 235;  	wait10Msec(100);  	liftPos(0);  	wait10Msec(500);  }else if(c > (a+b)/2){  	PlayImmediateTone(2024, 10);  	StopTask(getHeading);  	currHeading = 0;  	wait1Msec(200);  	StartTask(getHeading);  	wait1Msec(100);		turnDeg(-120, 25);		StopTask(getHeading);  	currHeading = 0;  	wait1Msec(200);  	StartTask(getHeading);		wait10Msec(70);  	forwardInc(-35, 30);//.........这里部分代码省略.........

task Rijden(){	 // Geluid dat de robot maakt tijdens het rijden, in loop.	k = 0; // Reset kruispuntboolean naar 0	//l = 0;	int var = 0; // variabele die naar een waarde wordt geset afhankelijk van welke sensor een signaal geeft.	while(true){		playSoundFile("hehe.rso");		// Linker sensor: Trigger wanneer de zwart-wit sensor (links) te weinig wit registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar links.		if(SensorValue[WIT] < 55)			var=1;		// Rechter sensor: Trigger wanneer de kleurensensor (rechts) zwart registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar rechts.		if((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))			var=2;		// kruispuntherkenning: Trigger wanneer zowel de kleurensensor als de zwartwit sensor getriggerd worden. Voer case 3 uit: stop, rij achteruit, wacht op commando.		if(((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))&&((SensorValue[WIT] <55)))			var=3;		// Tegenovergestelde van kruispuntherkenning: Trigger als de sensoren alleen wit registreren. Voer case 4 uit: rij naar voren.		if(((SensorValue[ZWART]!=blackcolor)&&(SensorValue[ZWART]!=greencolor))&&((SensorValue[WIT] >55)))			var=4;		// Obstakelsensor: Trigger wanneer de sonar een obstakel registreert. Voer case 5 uit: geef geluid en stop.		if (SensorValue[sonar] <30)			var=5;		switch(var)		{			case 1: //stuur naar links			setMotor(motorB,30);			setMotor(motorA,-5);			var=0;			k=0;			break;			case 2: //stuur naar rechts			setMotor(motorB,-5);			setMotor(motorA,30);			var=0;			k=0;			break;			case 3: //stoppen, ga achteruit, wacht op BlueTooth signaal			setMotor(motorB,0);			setMotor(motorA,0);			wait1Msec(100);			clearSounds();			setMotorTarget(motorA,85,-30); // ga achteruit			setMotorTarget(motorB,85,-30);			waitUntilMotorStop(motorA);			waitUntilMotorStop(motorB);			waitUntil(k==1); // wacht op een BlueTooth signaal dat links, rechts of vooruit zegt en k=1 triggert.			var=0;			k=0;			break;			case 4: // rij naar voren			setMotor(motorB,30);			setMotor(motorA,30);			var=0;			k=0;			break;			case 5: // als sonar wat ziet: schreeuw, rem rustig af en roteer 180*.			clearSounds();			playSoundFile("scream4.rso");			for (int i=3000;i>0 ; i--)			{				setMotor(motorA,i/100);				setMotor(motorB,i/100);			}			//waitUntil(l==1);			//nxtDisplayTextLine(1,"%d",i);			waitUntilMotorStop(motorA);			waitUntilMotorStop(motorB);			// oorspronkelijk hadden we code om een object te ontwijken, zowel linksom als rechtsom,			// met als alternatief om 180* te draaien. Dit kregen we echter niet op tijd volledig			// aan de praat, waardoor we hebben gekozen om hardcoded 180* te draaien.			// zie hiervoor de AvoidObject.c en AvoidObject.h files.			//			//AvoidMain();			//waitUntil(avoidDone==1);			//waitUntil(l==1);			//avoidDone = 0;			//			// einde ontwijkcode			// hardcoded rotate robot 180*			//setMotorTarget(motorA, 360, 30); // rechtsom draaien			//setMotorTarget(motorB, 360, -30);			//waitUntilMotorStop(motorA);			//waitUntilMotorStop(motorB);			// end hardcoded rotate robot 180*			setMotorTarget(motorA, 360, 30);			waitUntilMotorStop(motorA);			setMotorTarget(motorA, 135, 30);			setMotorTarget(motorB, 135, 30);			waitUntilMotorStop(motorA);			setMotorTarget(motorB, 360,30);			waitUntilMotorStop(motorB);//.........这里部分代码省略.........

void lock_msec(int speed, int duration){  setArmSpeed(speed);  wait1Msec(duration);  setArmSpeed(0);}

void turnRobot(float degrees, int direction, int turntype) {	float arc = degrees / 360;	float circumference, inches;	int ticks;	static int deltaLeft = 0, deltaRight = 0;	if (TIGHT_TURNS) {		circumference = PI * WHEEL_BASE;	}	else {		circumference = 2 * PI * WHEEL_BASE;	}	inches = circumference * arc;	ticks = inchesToTicks(inches);	nMotorEncoder[drive1] = 0;	nMotorEncoder[drive2] = 0;	if (PID_CONTROL == false) {		if (turntype == RIGHT) {			motor[drive2] = TURN_POWERL * direction;			if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE;		}		else {			motor[drive1] = TURN_POWERR * direction;			if (TIGHT_TURNS) motor[drive2] = TURN_POWERL * direction * REVERSE;		}		while (true) {			if (abs(nMotorEncoder[drive1]) >= ticks) break;			if (abs(nMotorEncoder[drive2]) >= ticks) break;		}	}	else {		if (turntype == RIGHT) {			nMotorEncoderTarget[drive2] = ticks * direction;			if (TIGHT_TURNS) nMotorEncoderTarget[drive1] = ticks * direction * REVERSE;			motor[drive2] = TURN_POWERL * direction;			if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE;		}		else {			nMotorEncoderTarget[drive1] = ticks * direction;			if (TIGHT_TURNS) nMotorEncoderTarget[drive2] = ticks * direction * REVERSE;			motor[drive1] = TURN_POWERL * direction;			if (TIGHT_TURNS) motor[drive2] = TURN_POWERR * direction * REVERSE;		}		while (true) {			TNxtRunState state1, state2;			// 0 = runStateIdle			// 1 = runStateHoldPosition			// 16 = runStateRampUp			// 32 = runStateRunning			// 64 = runStateRampDown			state1 = nMotorRunState[drive1];			state2 = nMotorRunState[drive2];			if (state1 == runStateIdle && state2 == runStateIdle) break;		}	}	motor[drive1] = 0;	motor[drive2] = 0;	metricsDisplay(ticks, ticks);	deltaLeft = nMotorEncoder[drive2] - ticks;	deltaRight = nMotorEncoder[drive1] - ticks;	nMotorEncoder[drive1] = 0;	nMotorEncoder[drive2] = 0;	if (PLAY_SOUNDS == true) {		PlaySound(soundBeepBeep);	}	wait1Msec(DRIVE_DELAY);	return;}

void drive_msec(int speed, int duration) {  setDriveSpeed(speed);  wait1Msec(duration);  killdrive;}

task main () {  bool selected_50hz = true;  nxtDisplayCenteredTextLine(0, "HiTechnic");  nxtDisplayCenteredBigTextLine(1, "Color V2");  nxtDisplayCenteredTextLine(4, "Config operating");  nxtDisplayCenteredTextLine(5, "frequency to");  nxtDisplayCenteredTextLine(6, "50 or 60 Hz");  wait1Msec(2000);  eraseDisplay();  nxtDisplayCenteredTextLine(0, "Use arrow keys");  nxtDisplayCenteredTextLine(1, "to select a");  nxtDisplayCenteredTextLine(2, "frequency");  nxtDisplayCenteredBigTextLine(4, "50 60");  nxtDisplayCenteredTextLine(6, "[enter] to set");  nxtDisplayCenteredTextLine(7, "[exit] to cancel");  nxtDrawRect(19, 34, 44, 16);  while (true) {    // Do nothing while no buttons are pressed    while (nNxtButtonPressed == kNoButton) {      wait1Msec(1);    }    switch (nNxtButtonPressed) {      // if the left button is pressed, set the sensor for 50Hz      case kLeftButton:            if (selected_50hz) {              PlaySound(soundBlip);              while(bSoundActive) {wait1Msec(1);}            } else {              selected_50hz = true;              nxtEraseRect(55, 34, 80, 16);              nxtDisplayCenteredBigTextLine(4, "50 60");              nxtDrawRect(19, 34, 44, 16);            }            break;       // if the right button is pressed, set the sensor for 60Hz       case kRightButton:						if (!selected_50hz) {						  PlaySound(soundBlip);						  while(bSoundActive) {wait1Msec(1);}						} else {						  selected_50hz = false;						  nxtEraseRect(19, 34, 44, 16);						  nxtDisplayCenteredBigTextLine(4, "50 60");						  nxtDrawRect(55, 34, 80, 16);						}            break;        // Make the setting permanent by saving it to the sensor        case kEnterButton:            eraseDisplay();            nxtDisplayCenteredTextLine(2, "The Sensor is");            nxtDisplayCenteredTextLine(3, "configured for");            if (selected_50hz) {              _HTCSsendCommand(HTCS2, 0x35);              nxtDisplayCenteredTextLine(4, "50 Hz operating");            } else {              _HTCSsendCommand(HTCS2, 0x36);              nxtDisplayCenteredTextLine(4, "60 Hz operating");            }            nxtDisplayCenteredTextLine(5, "frequency");            for (int i = 5; i > 0; i--) {              nxtDisplayCenteredTextLine(7, "Exiting in %d sec", i);              wait1Msec(1000);            }            StopAllTasks();            break;    }    // Debounce the button    while (nNxtButtonPressed != kNoButton) {      wait1Msec(1);    }  }}

// update motor speed and wait for durationvoid control(int left, int right, int duration = 0){    motor[Left] = left;    motor[Right] = right;    if(duration)        wait1Msec(duration);}

/*-----------------------------------------------------------------------------*/voidiqMenuConfiguration(){    static  tConfigSelect configSelect = kConfig_exit;    static int offset = 0;    bool done   = false;    bool update = true;    // Wait for LCD button release    while( nLCDButtons != kButtonNone )    wait1Msec(10);    eraseDisplay();    while( !done ) {        // Display menu        if( update ) {            iqMenuConfigurationDisplay( configSelect, offset );            update = false;        }        // Check LCD buttons        if( nLCDButtons != kButtonNone ) {            if( nLCDButtons == kButtonDown  ) {                if( configSelect++ == kConfig_6 )                    configSelect = kConfig_6;                if( configSelect > kConfig_3 && offset == 0 )                    offset = 3;            }            if( nLCDButtons == kButtonUp  ) {                if( configSelect-- == kConfig_exit )                    configSelect = kConfig_exit;                if( configSelect < kConfig_2 && offset == 3 )                    offset = 0;            }            if( nLCDButtons == kButtonSelect  ) {                if( iqMenuConfigurationSelect( configSelect ) )                    done = true;            }            // Exit doesn't really work            if( nLCDButtons == kButtonExit  ) {                done = true;            }            // Wait for LCD button release            while( nLCDButtons != kButtonNone )                wait1Msec(10);            // update display            update = true;        }    wait1Msec(25);    }    // erase before we leave this sub menu    eraseDisplay();}