QDir users(const QString &prefixHash)
 {
     return drive(prefixHash).absoluteFilePath("users");
 }

task main()
{
	infra ();
	drive () ;

}

//.........这里部分代码省略.........
			 case KNIFE:
			 case KILL:
				murder();
				break;

			 case UNDRESS:
			 case RAVAGE:
				ravage();
				break;

			 case SAVE:
				save();
				break;

			 case FOLLOW:
				lflag = follow();
				break;

			 case GIVE:
				give();
				break;

			 case KISS:
				kiss();
				break;

			 case LOVE:
				 love();
				 break;

			 case RIDE:
				lflag = ride();
				break;

			 case DRIVE:
				lflag = drive();
				break;

			 case LIGHT:
				 light();
				 break;

			 case LAUNCH:
				if (!launch())
					return(-1);
				else 
					lflag = 0;
				break;

			case LANDIT:
				if (!land())
					return(-1);
				else
					lflag = 0;
				break;

			case TIME:
				chime();
				break;

			 case SLEEP:
				zzz();
				break;

			 case DIG:
				dig();
				break;

			 case JUMP:
				lflag = jump();
				break;

			 case BURY:
				bury();
				break;

			 case SWIM:
				puts("Surf's up!");
				break;

			 case DRINK:
				drink();
				break;

			 case QUIT:
				die();

			 default:
				puts("How's that?");
				return(-1);
				break;

			 
		}
		if (wordnumber < wordcount && *words[wordnumber++] == ',')
			continue;
		else return(lflag);
       }
       return(lflag);
}

void Mobot::navigate(int p1x, int p1y, int p2x, int p2y) {
  // Assume that only one of dx or dy is nonzero and that their value is
  // either -1 or +1. Same goes for headingX and headingY.

  //             + -y
  //             |
  //             |
  //             |
  //     <--------------->
  //     -x      |      +x
  //             |
  //             |
  //             + +y
  //
  // Counterclockwise: positive degrees
  // Clockwise: negative degrees

  int dx = p2x - p1x;
  int dy = p2y - p1y;

  int turnDegrees = 0;

  if (dx != 0) {
    // If in the opposite direction, turn around
    if (dx * headingX < 0) {
      turnDegrees = 180;
    }

    else if (dx * headingY > 0) {
      turnDegrees = 90;
    }

    else if (dx * headingY < 0) {
      turnDegrees = -90;
    }
  }

  else if (dy != 0) {
    // If in the opposite direction, turn around
    if (dy * headingY < 0) {
      turnDegrees = 180;
    }

    else if (dy * headingX > 0) {
      turnDegrees = -90;
    }

    else if (dy * headingX < 0) {
      turnDegrees = 90;
    }
  }

  headingX = dx;
  headingY = dy;

  if (turnDegrees != 0) {
    turn(turnDegrees);
  }

  drive(resolution);
}

void ScrollMachine::wheel(int steps)
{
	speed_ += steps * trans_;
	if (int(speed_) == 0) speed_ = steps;
	drive();
}

task main()
{
	waitForStart();
	//servoChangeRate[handJoint] = 10;
	nMotorEncoder[spear] = 0;
	writeDebugStreamLine("encoder set to: %d", nMotorEncoder[spear]);

	int maxVal = 40;

	while (true)
	{
		getJoystickSettings(joystick);

		int cont1_left_yval = avoidWeird(joystick.joy1_y1, 20); //y coordinate for the left joystick on controller 1
		int cont1_left_xval = avoidWeird(joystick.joy1_x1, 75); //x coordinate for the left joystick on controller 1
		int cont1_right_yval = avoidWeird(joystick.joy1_y2, 20);
		int cont1_dPad = joystick.joy1_TopHat; //Value of the dPad for controller 2

		//if (joy1Btn(4) == 1)
		//{
		//	if ((ServoValue[handJoint] + 5) < maxHandValue)
		//	{
		//		servo[handJoint] = ServoValue[handJoint] + 5;
		//	}
		//}
		//if (joy1Btn(2) == 1)
		//{
		//	if ((ServoValue[handJoint] - 5) > minHandValue)
		//	{
		//		servo[handJoint] = ServoValue[handJoint] - 5;
		//	}
		//}

		if (joy1Btn(1) == 1){
			nMotorEncoder[spear] = 0;
		}

		if (joy1Btn(4) == 1){
			spearMovement(20);
		}
		if (joy1Btn(2) == 1){
			spearMovement(-20);
		}
		if (joy1Btn(2) != 1 && joy1Btn(4) != 1){
			spearMovement(0);
		}

		//if (joy1Btn(6) == 1)
		//{
		//	fold_arm(false);
		//}
		//if (joy1Btn(5) == 1)
		//{
		//	fold_arm(true);
		//}

		if (joy1Btn(1) == 1){
			maxVal = 100;
		}

		if (joy1Btn(1) != 1){
			maxVal = 40;
		}

		//if (joy2Btn(1) == 1)
		//{
		//	servo[ramp] = 0;
		//}
		//if (joy2Btn(3) == 1)
		//{
		//	servo[ramp] = 255;
		//}
		//if (joy2Btn(1) != 1 && joy2Btn(3) != 1)
		//{
		//	servo[ramp] = 128;
		//}

		drive(cont1_left_yval, cont1_left_xval, maxVal);
		//shoulderMovement(cont1_right_yval);
		//handMovement(cont1_dPad);
	}
}

int main ( void )
{
  uint8_t *bufcontents;
  uint8_t i;
  uint8_t tv[] = "foobar1";
  uint16_t ticker = 0;
  uint8_t rc;
  aes128_ctx_t ctx;

  uint8_t key[] = "0123456789ABCDEF";
  uint8_t IV[] = "FEDCBA9876543210";
  uint8_t text[] = "Hello rfm12 world. I've gonna cipher you       ";
	
  drive(LED1);
  drive(LED2);

  toggle_output(LED1);
	
  uart_init();

  _delay_ms(250);
  _delay_ms(250);

  sei();

  toggle_output(LED2);
  uart_putstr ("AVR Boot Ok\r\n");
  _delay_ms(250);
  toggle_output(LED2);

  aes128_init(key,&ctx);

  while (1) {
    uart_putstr("key = ");
    uart_putstr(key);
    uart_putstr("\r\n");

    uart_putstr("text = ");
    uart_putstr(text);
    uart_putstr("\r\n");

    /* Ciphering in CBC mode */
    memxor(text,IV,16);
    aes128_enc(text,&ctx);
    memxor(text+16,text,16);
    aes128_enc(text+16,&ctx);
    memxor(text+32,text+16,16);
    aes128_enc(text+32,&ctx);

    uart_putstr("text ciphered = \r\n");
    uart_hexdump(text,sizeof(text));

    /* Deciphering in CBC mode */
    aes128_dec(text+32,&ctx);
    memxor(text+32,text+16,16);
    aes128_dec(text+16,&ctx);
    memxor(text+16,text,16);
    aes128_dec(text,&ctx);
    memxor(text,IV,16);

    uart_putstr("text unciphered = ");
    uart_putstr(text);
    uart_putstr("\r\n");

    /* Let change the IV */
    IV[0]++;

    _delay_ms(250);
    _delay_ms(250);
    _delay_ms(250);
    _delay_ms(250);
  }
}

/*
 * Runs the user operator control code. This function will be started in its own task with the
 * default priority and stack size whenever the robot is enabled via the Field Management System
 * or the VEX Competition Switch in the operator control mode. If the robot is disabled or
 * communications is lost, the operator control task will be stopped by the kernel. Re-enabling
 * the robot will restart the task, not resume it from where it left off.
 *
 * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
 * run the operator control task. Be warned that this will also occur if the VEX Cortex is
 * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
 *
 * Code running in this task can take almost any action, as the VEX Joystick is available and
 * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
 * recommended to give other tasks (including system tasks such as updating LCDs) time to run.
 *
 * This task should never exit; it should end with some kind of infinite loop, even if empty.
 */
void operatorControl() {
#ifdef AUTO
	autonomous();
#elif defined(TEST)

#define DEFAULT_SHOOTER_SPEED 0
#define SHOOTER_SPEED_INCREMENT 5

	int8_t xSpeed, ySpeed, rotation;
	int8_t lifterSpeed/*, intakeSpeed*/;

	int16_t shooterSpeed = DEFAULT_SHOOTER_SPEED; //shooter is on when robot starts
	int8_t frontIntakeSpeed = INTAKE_SPEED;
	bool isShooterOn = true;
	bool isAutoShootOn = false;

	//lfilterClear();

	toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP);		// intake forward
	toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT);	// intake off
	toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT);	// intake off
	toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN);	// intake backward
	toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN);   // shooter on off
	toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT);   // auto shoot on off
	toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP);   // shooter speed up
	toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN);   // shooter speed down

	while (true) {
		printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54);

		toggleBtnUpdateAll();

		// drive
		xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS);
		ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS);
		rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2;

		if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) {
			ySpeed = 0;
		}

		if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) {
			xSpeed = 0;
		}

		drive(xSpeed, ySpeed, rotation, false);

		// lifter up down
		if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) {
			lifterSpeed = LIFTER_SPEED;
		} else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) {
			lifterSpeed = -LIFTER_SPEED;
		} else {
			lifterSpeed = 0;
		}

		lifter(lifterSpeed);
		takeInInternal(lifterSpeed);

		if (isShooterOn) {
			if (isAutoShootOn) {
				shooterSpeed = calculateShooterSpeed();
			} else {
				// shooter increase speed
				if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
					shooterSpeed += SHOOTER_SPEED_INCREMENT;

					if (shooterSpeed > SHOOTER_MAX_SPEED) {
						shooterSpeed = SHOOTER_MAX_SPEED;
					}
				}

				// shooter decrease speed
				if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
					shooterSpeed -= SHOOTER_SPEED_INCREMENT;

					if (shooterSpeed < SHOOTER_MIN_SPEED) {
						shooterSpeed = SHOOTER_MIN_SPEED;
					}
				}
			}

//			 auto shooter on off
//.........这里部分代码省略.........

void DifferentialPilot::drive
    (Robot* rob, Point goalPoint, float theta_goal, MoveType moveType)
{
    return drive(rob, goalPoint.x, goalPoint.y, theta_goal, moveType);
}

void Robot::writeMotorsSpeed(int motorLeft, int motorRight)
{
	motorLeft = (float)motorLeft * motorsDiffMultiplier;

	drive(motorLeft, motorRight);
}

void AFK_Camera::driveAndUpdateProjection(const Vec3<float>& velocity, const Vec3<float>& axisDisplacement)
{
    drive(velocity, axisDisplacement);
    updateProjection();
}

task main() {
    //Code: drive(auto_command,time);
    //replace auto_command and time with values necessary (if you don't replace them, code won't run)
    //auto_command values are: "rpoint", "lpoint", "up", "down", "rswing", "lswing", "rswingback", "lswingback"
    //time values is the time, in milliseconds, you want the robot to be doing the action specified in auto_command

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


    waitForStart();

    /*
    int count = 0;

    string BatteryLevel = externalBatteryAvg;
    string selection = "";


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


//Drive forward off of home red cliff
    drive(0,4596);
    drive(65535,100);

//Turn left 90 degrees
    drive(6,3185);
    drive(65535,100);

//Drive forward to bowling ball
    drive(0,1036);
    drive(65535,100);

//Turn left 90 degrees
    drive(6,4037);
    drive(65535,100);

//Turn left a bit
    drive(6,516);
    drive(65535,100);
    drive(0,657);//Drive past bowling ball
    drive(65535,100);
    drive(2,188);//Turn right into bowling ball
    drive(65535,100);
    drive(2,237);//Turn right into bowling ball
    drive(65535,100);
    drive(0,425);//Drive forward
    drive(65535,100);
    drive(2,279);//Turn right into bowling ball
    drive(65535,100);
    drive(2,424);//Turn right into bowling ball
    drive(65535,100);
    drive(0,799);//Drive forward a bit
    drive(65535,100);
    drive(2,468);//Turn right into bowling ball
    drive(65535,100);
    drive(0,516);//Drive forward a bit
    drive(65535,100);
    drive(2,2065);//Turn right into bowling ball
    drive(65535,100);
    drive(0,1409);//Drive forward
    drive(65535,100);
    drive(2,8299);//Turn right, line up with bowling ball
    drive(65535,100);
    drive(2,424);//Turn right, line up with bowling ball

//Drive bowling ball into front zone
    drive(65535,100);
    drive(0,4408);//Drive forward
    drive(65535,100);
    drive(2,424);//Realign with bowling ball
    drive(65535,100);
    drive(0,2908);//Drive forward
    drive(65535,100);
    drive(2,608);//Realign with bowling ball
    drive(65535,100);
    drive(0,1312);//Drive forward
    drive(0,1920);//Drive forward
    drive(65535,100);
    drive(0,424);//Drive forward
    drive(65535,100);

    drive(4,9756);//Drive backwards
    drive(65535,100);
    drive(4,376);//Drive backwards
    drive(4,3516);//Drive backwards

}

task main()
{
	bDisplayDiagnostics=false;
	nMotorEncoder[sense]=0;
	/*
	motor[sense]=5;
	while(SensorValue[ir]!=5);
	motor[sense]=0;
	nxtDisplayCenteredBigTextLine(0,"%d",nMotorEncoder[sense]);
	motor[sense]=-5;
	while(nMotorEncoder[sense]>0);
	motor[sense]=0;
	wait1Msec(100000);
	StopAllTasks();
	*/

	init();
	theta=0;
	//downRamp();
	waitForStart();
	//wait1Msec(13000);
	StartTask(gyroThread);
	StartTask(graph);
	StartTask(grabber);
	//wait1msec(2000);
	//servo[bucket]=160;
//wait1Msec(100000);
	/*
forwardToLine();
	rotateLeft(20,20);
	drive(0);
	wait1Msec(250);
//	forwardToTube();
*/
//	servo[grab]=210;

//	wait1Msec(200);
//	bringTubeBack();


//servo[grab]=255;
//wait1Msec(1000);
//drive(50);
//wait1msec(2500);
if(STARTINGPOS==0){
	drive(0);
	grabState=2;
	forwardToTube2();
	bringTubeBack();
	wait1Msec(1000);
	//dump();
}
else{
	grabState=1;
	drive(50);
	wait1Msec(2000);
	drive(30);
	grabState=0;
	wait1Msec(1000);
	forwardToTube2();
	backToZone();
}
//backToZone();
//		while(true)readAnalogInput(HTPB,0);//before edge
	//downRamp();
	//clearTimer(T1);
	/*
	motor[claw]=50;
	wait1Msec(250);
	motor[claw]=5;
	motor[right]=50;
	motor[left]=0;
	wait1Msec(1000);
	motor[left]=-50;
	wait1Msec(8000);
	drive(0);*/
	while(true);
}

void timedDrive(int direction, float seconds) {
	drive(direction);
	Wait(seconds);
	drive(NO_WHERE);
}

void backwardTouch(int speed)
{
    drive(speed);
    while(!TOUCH_BACK_OR);
    create_stop();
}

task main(){
	//Code: drive(auto_command,time);
	//replace auto_command and time with values necessary (if you don't replace them, code won't run)
	//auto_command values are: "rpoint", "lpoint", "up", "down", "rswing", "lswing", "rswingback", "lswingback"
	//time values is the time, in milliseconds, you want the robot to be doing the action specified in auto_command

	//Info:
	//Motor at 75% power travels at 2 feet per second = 60.96 cm
	//Lpoint turn takes 1560 ms to turn 90 degrees
	//Ultrasonic sensor is 9cm away from back

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

	string program_type="Red";
	int offset_back = 0;
	int offset_right = 0;
	int robot_length = 667; //In milliseconds
	int distance = 0;


	if (program_type=="Red") {
		//1) Measure offset from walls
		//!IMPORTANT START ROBOT SIDEWAYS facing left to reduce time it takes to measure
		//!IMPORTANT Ultrasonic measurer should be placed facing the back with the edge lined up with the robot's back
		//Convert offset distance into milliseconds

	  //Measure
		offset_right = (SensorValue[sonarSensor] - 9)/60.96;
		// Turn right 90 degreees
		distance = 1560;
		drive("rpoint", distance);
		//Remeasure
		offset_back = (SensorValue[sonarSensor] - 9)/60.96;


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

		distance = 2291.66666 - offset_back;
		drive("up", distance);

		//3) Point turn -90 degrees

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

		//4) Move forward ((Four Feet Eight Inches=142.24 cm)-(the robot's length)-offset_right)

		distance = 2333.333333 - robot_length - offset_right;
		drive("up", distance);

		//5) Point turn -33.7 degrees

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

		//6) Move forward ((Nine Feet Two Inches=279.4 cm)-(the robot's length))

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

		//7) Pause 1 second

		wait1Msec(1000);

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

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

		//9) Point turn 33.7 degrees

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

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

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

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

		//12) Point Turn 90 degrees

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

		//13) Measure offset
		offset_back = (SensorValue[sonarSensor] - 9)/60.96;

//.........这里部分代码省略.........

void forwardTouch(int speed)
{
    drive(-speed);
    while(!TOUCH_FRONT);
    create_stop();
}

task usercontrol ()
{
	while(1){
	drive();
}
}

/*lint -save  -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
  /* Write your local variable definition here */

	bool button=1;

	struct sensor s[6];
	
	int64_t min_avg[6]={23, 14, 15, 15, 18, 23};
	int64_t max_avg[6]={1000, 193, 172, 170, 160, 550};
	int i;
	int64_t error;

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  /* Write your code here */
  //Initializeaza timer-ul
  CountTimer_Init((LDD_TUserData *)NULL);
  //Opreste motoarele
  motor(0, 0);
  //Activeaza PWM-ul pentru motoare
  PWM_stanga_Enable();
  PWM_dreapta_Enable();
  
  IR_LED_PutVal(0);
  while(button)
	button=Button_GetVal();
  WAIT1_Waitms(500);
  IR_LED_PutVal(1);

  while(1){
	  
	  readSensors(min_avg, max_avg, s);
	  WAIT1_Waitms(1);
#if DEBUG==TRUE
	  for(i=0; i<6; i++){
		  Term1_SendNum(s[i].value);
		  Term1_SendStr("  ");
	  }
	  for(i=0; i<6; i++){
		  Term1_SendNum(s[i].seen);
		  Term1_SendStr("  ");
	  }
#endif
	  error=propder(s);
#if DEBUG==TRUE
	  Term1_SendNum(error);
	  Term1_SendChar('\n');
	  Term1_SendChar('\r');
#endif
	  drive(error);

  }




  /*** Don't write any code pass this line, or it will be deleted during code generation. ***/
  /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
  #ifdef PEX_RTOS_START
    PEX_RTOS_START();                  /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
  #endif
  /*** End of RTOS startup code.  ***/
  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

void Robot::startActing()
{
	std::thread newRobotThread( [this]
	{	stop = false; drive();});
	robotThread.swap( newRobotThread);
}