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

	task drive()	{		if(abs(vexRT[Ch2]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)		{			motor[DFR]=vexRT[Ch2];			motor[DBR]=vexRT[Ch2];			motor[DFL]=vexRT[Ch2];			motor[DBL]=vexRT[Ch2];		}		else		{			motor[DFR]=0;			motor[DBR]=0;			motor[DFL]=0;			motor[DBL]=0;		}		if(abs(vexRT[Ch1]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)		{			motor[middle]=vexRT[Ch1];		}		else		{			motor[middle]=0;		}		if(abs(vexRT[Ch4]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)		{			turnRight(127);		}		else		{			turnRight(0);		}	}

task main(){	forward(2*12/WHEEL_CIRCUM);	doNothing(1000);	turnLeft(0.8);	doNothing(1000);	forward(2.5*12/WHEEL_CIRCUM);	doNothing(1000);	turnRight(0.9);	doNothing(1000);	forward(1.5*12/WHEEL_CIRCUM);	doNothing(1000);	turnRight(0.8);	doNothing(1000);	openClaw();	doNothing(1000);	forward(12/WHEEL_CIRCUM);	doNothing(1000);	closeClaw();	doNothing(1000);	moveArmUp(900);	doNothing(1000);	turnLeft(1.6);	forward(12/WHEEL_CIRCUM);	turnLeft(0.8);	forward(1.5*12/WHEEL_CIRCUM);	turnLeft(0.8);	forward(2.5*12/WHEEL_CIRCUM);	turnLeft(0.8);	forward(2*12/WHEEL_CIRCUM);	moveArmDown(900);	openClaw();	backward(4*12/WHEEL_CIRCUM);}

void terminalCheck1(){	if (ct != ot)	{		if (dir == 1 || dir == 3)		{			if (ot == 3 || ot == 0)			turnRight();			else turnLeft();		}		if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))		turn();		front();		ot = ct;	}	else		turnRight();	//lcd_print(1,1,1111,4);	//printf("Enter term[%d][%d]/n", ct, 0);	//scanf("%d", &term[ct][0]);	term[ct][0]=scan();	lcd_print(2,1,term[ct][0],1);//	lcd((char*)term[ct][0]);	if (term[ct][0] == -1 || term[ct][0] == color[ct])	total--;}

void rejectionRed(){	// variables		int armMin = 300;		int wallHeight = 1000;		int goalHeight = 1550;		int pot = analogRead (8);		int encoder1 = 1200; //drive under wall		int encoder2 = 136; // rotate 90 degrees		int encoder3 = 900; // backwards to the opponets goal		int encoder4 = 150; //rotate 90		int encoder5 = 200; // ass towards bridge		// begin routine		intakeDead(); // unfold		delay(300); // needs to clear the wall..		driveBack (encoder1); // drive backwards to under the bridge		stopIntake();		turnRight (encoder2); // turn ass to opponets goal		driveBack (encoder3); // drive to opponets goal		turnRight (encoder4); // ass to bridge		armUp(wallHeight);// arm up		driveBack(encoder5); // block their launch		stopAll ();}

/*1 dist unit = 1 second at speed 100 with standard wheels;*/int testTurnSpeed(){ //account for the distance between the wheels, etc. Returns a "dist" number which represents a 360 degree turn. Speed of turn * length of turn = value returned. 	//Thus, it takes X number of units (X determined here), to turn 360 degrees.	int currentPosition = 0;	int FIRST_SPEED = 250; //speed at which for the first portion.	int TIMEINTERVAL = 40;	printf("Please press A when the Robot has turned 90 degrees. /n");	while(a_button_clicked()==0){ //turn a bit past 90 degrees.		turnRight(FIRST_SPEED, TIMEINTERVAL);		currentPosition = currentPosition + (FIRST_SPEED*TIMEINTERVAL);	}		printf("Moving back a bit. /n");	int toMoveBack = currentPosition/5; //move back 1/5 of the distance.	turnLeft(FIRST_SPEED, toMoveBack/FIRST_SPEED);		printf("Please press A when the Robot is back at 90 degrees. /n");	while(a_button_clicked()==0){ //turn to 90 degrees at 1/4 previous speed.		turnRight(FIRST_SPEED/4, TIMEINTERVAL);		currentPosition = currentPosition + (FIRST_SPEED/4*TIMEINTERVAL);	}		printf("Current Value: %d /n Will now turn 360 degrees. /n", currentPosition*4);		turnRight(500, currentPosition*4/500); //make 360 degree turn at speed 300.		printf("Button A: Done /n Button B: Redo /n");	int button = getFirstButtonPressed();	if(button == 0){		printf("360 degree turn value: %d /n", (currentPosition*4));		} else if(button == 1){		return testTurnSpeed();	} else return -1;	}

void roboControl(void){ //For autonomous control	setLauncherSpeed(30);	launcherSpeed_new = 127;//Lets start this shoot speed	goForwardFor_time(10);//Go forward 10 seconds	goBackwardFor_time(1);	clearTimer(T1);	while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}	/*goBackwardFor_time(1);	turnRight(0.1);	setMotor(hozBelt,-100);	goForwardFor_time(.5);	turnRight(0.1);	goForwardFor_time(5);*/	goForwardFor_time(7);//Go forward 7 seconds	goBackwardFor_time(1); //Get to launching distance	clearTimer(T1);	while(time1[T1] < 15000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}	goBackwardFor_time(1);	turnRight(1);	setMotor(hozBelt,-100);	goForwardFor_time(.5);	turnRight(0.25);	goForwardFor_time(5);}

void traverseToSort(int a, int b){	if (flag == 1)		flag = 0;	if (a == 4 || a == 5)	{		if ((a == 4 && dir == 0) || (a == 5 && dir == 2))			turnRight();		else turnLeft();		front();		front();		if (a == 4)			ot = 5;		else ot = 4;		cost = cost + 2;	}	else{		if ((dir == 1 && (ot == 0 || ot == 1)) || (dir == 3 && (ot == 2 || ot == 2)))		{			turnRight();		}		else if ((dir == 3 && (ot == 0 || ot == 1)) || (dir == 1 && (ot == 2 || ot == 2)))		{			turnLeft();		}		if ((a <= 1 && dir == 0) || (a >= 2 && dir == 2))			turn();		front();		ot = ct % 2 + 4;	}}

void moveCar(orientation direction, int distance){  orient(distance);   switch(direction){  	case Straight:  	  driveStraight(distance);  	break;  	case Right:  	  turnRight(distance);	driveStraight(distance);  	break;  	case Left:  	  turnLeft(distance);	driveStraight(distance);  	break;	case Back:		turnRight(distance);		sleep(1);		turnRight(distance);		driveStraight(distance);		break;  	default:  	  motorright.duty = 7.25;  	  motorleft.duty = 7.25;  	  break;   }  //bcm2835_close();  return ;}

task main(){  initializeRobot();  waitForStart(); // Wait for the beginning of autonomous phase.  moveForward((1400*2),35);  servo[left1Scoop] = 6;  servo[left2Scoop] = 6;  servo[right1Scoop] = 6;  servo[right2Scoop] = 6;  moveForward((1400*0.5), -35);  servo[left1Scoop] = 46;  servo[left2Scoop] = 46;  servo[right1Scoop] = 46;  servo[right2Scoop] = 46;  moveForward((1400*1.5), -35);  turnRight((1100),35);  servo[left1Scoop] = 46;  servo[left2Scoop] = 46;  servo[right1Scoop] = 46;  servo[right2Scoop] = 46;  wait1Msec(500);  moveForward((1400),-25);  wait1Msec(500);  turnLeft((1100),40);  wait1Msec(500);  moveForward((1400*4),-40);  wait1Msec(500);  turnRight((1100),40);  wait1Msec(500);  moveForward((1400*2),-40);  wait1Msec(500);  turnRight((1100),40);}

void armbotRed(){	int wallHeightU = 300; // idk	int wallHeight = 1000; // idk	int floorHeightU = 1700; // idk	int floorHeight = 1600;	int pot = analogRead (8);	int halfDist = 900; // encoder value	armUpEnc(wallHeightU);	turnRight(halfDist);	armDownTime (300);	armUpEnc(wallHeightU); // pick it up	turnLeft(halfDist);	//armDown(floorHeightU);	turnLeft(halfDist);	armDownTime (300); // score the spike!	armUpEnc(floorHeightU);	turnRight(halfDist);	//////////////////////////////////// and loop/////////////////////////////	//end of routine	stopAll () ;	delay(60000);///////////////////////////////////////////////////////////////////////////////////}

// Center of treevoid centerBranch(int depth) {    int terminatingBranch = 0;    if (depth == 0) {        translateBy(0, 5 * (1.0 / scaleFactor), 0);        terminatingLeaf();        return;    } // if        if (depth == 1) {        terminatingBranch = TERM;    } // if        translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(CENTER, depth);        // Left branch    pushMatrix();    turnLeft();    translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(LEFT | terminatingBranch, depth);    leftBranch(depth - 1);    popMatrix();        // 3D left branch    pushMatrix();    rotateBy(90, 0, 1, 0);    rotateBy(15, 0, 0, 1);    translateBy(1, 0, 0);    turnLeft();    translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(LEFT | terminatingBranch, depth);    leftBranch(depth - 1);    popMatrix();        translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(CENTER, depth);        // Right branch    pushMatrix();    turnRight();    translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(RIGHT | terminatingBranch, depth);    rightBranch(depth - 1);    popMatrix();        // 3D right branch    pushMatrix();    rotateBy(90, 0, 1, 0);    rotateBy(-15, 0, 0, 1);    translateBy(-1, 0, 0);    turnRight();    translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(RIGHT | terminatingBranch, depth);    rightBranch(depth - 1);    popMatrix();        translateBy(0, 6 * (depth * 1.0 / scaleFactor), 0);    drawBranch(CENTER | TERM, depth);    centerBranch(depth - 1);} // centerBranch

void sierpinski(turtle* t, int len, int iters){    int part;        part = len/3;        if(iters <=1 ){        turnLeft(t, 60);        makeLine(t, part);                turnRight(t, 60);        makeLine(t, part);                turnRight(t, 60);        makeLine(t, part);                turnLeft(t, 60);        return;    }        turnLeft(t, 60);    mirrorTurtle(t);    sierpinski(t, part, iters-1);    mirrorTurtle(t);        turnRight(t, 60);    sierpinski(t, part, iters-1);        turnRight(t, 60);    mirrorTurtle(t);    sierpinski(t, part, iters-1);    mirrorTurtle(t);        turnLeft(t, 60);}

void drawTau(Servo *leftWheel, Servo *rightWheel, Servo *penServo){    //DRAWING THE SERIF MARKS AND TOP PART OF TAU  penDown(leftWheel, rightWheel, penServo);   //Put pen down to start drawing  backward(leftWheel, rightWheel, DISTANCE*.09); //drawing left serif  forward(leftWheel, rightWheel, DISTANCE*.09); //Move to initial point       turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES); //Turn to draw top of T  forward(leftWheel, rightWheel, DISTANCE*.293 + 2*(DISTANCE*.207));  //drawing top part of T    turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);  forward(leftWheel, rightWheel, DISTANCE*.09);  //drawing right serif  backward(leftWheel, rightWheel, DISTANCE*.09);    //Now we want to move it to the center of T to continue drawing middle part of Tau  //penUp(leftWheel, rightWheel, penServo);    //penUp so it doesn't leave a trail    turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);  penUp(leftWheel, rightWheel, penServo);  //penUp so it doesn't leave a trail  forward(leftWheel, rightWheel, (DISTANCE*.293 + 2*(DISTANCE*.207)) / 2); // Move to the top center of Tau    penDown(leftWheel, rightWheel, penServo);    //put pen down to draw again  turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES - 70);     //turn 90 degrees to get ready to draw bottom Tau  forward(leftWheel, rightWheel, DISTANCE - 80);     //DRAW BOTTOM PART OF TAU  turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);  forward(leftWheel, rightWheel, DISTANCE*.073);  backward(leftWheel, rightWheel, DISTANCE*.135);  penUp(leftWheel, rightWheel, penServo);  //completed drawing Tau, pen up.   stopBot(leftWheel, rightWheel, DISTANCE);  //finished drawing, freeze for ten seconds.   }

task main(){	//ALWAYS START CHALLENGE WITH CLAW CLOSED!!!	forward((2 * 12 - 0.5) / WHEEL_CIRCUM);	doNothing(1000);	turnLeft(0.8);	doNothing(1000);	forward((2.4 * 12 + 1) / WHEEL_CIRCUM);	doNothing(1000);	turnLeft(0.75);	doNothing(1000);	openClaw();	doNothing(1000);	forward(13.25 / WHEEL_CIRCUM);	doNothing(1000);	closeClaw();	doNothing(1000);	moveArmUp(900);	doNothing(1000);	turnLeft(1.5);	doNothing(1000);	forward((2.7 * 12 + 3) / WHEEL_CIRCUM);	doNothing(1000);	turnRight(0.7);	doNothing(1000);	moveArmUp(200);	doNothing(1000);	forward(2.5 / WHEEL_CIRCUM);	doNothing(1000);	openClaw();	doNothing(1000);	moveArmUp(300);	doNothing(1000);	backward(2 / WHEEL_CIRCUM);	doNothing(1000);	moveArmDown(600);	doNothing(1000);	forward(2.5 / WHEEL_CIRCUM);	doNothing(1000);	closeClaw();	doNothing(1000);	moveArmUp(900);	doNothing(1000);	turnRight(0.7);	doNothing(1000);	forward(1.5 * 12 / WHEEL_CIRCUM);	doNothing(1000);	turnLeft(0.7);	doNothing(1000);	forward(2 * 12 / WHEEL_CIRCUM);	doNothing(1000);	turnLeft(0.7);	doNothing(1000);	forward(12 / WHEEL_CIRCUM);	doNothing(1000);	moveArmDown(900);	doNothing(1000);	openClaw();}

int main(){    courses direction=FRONT;    /* Direction of the character */    int     currentPosition=START_POSITION, /* Position of the character */            step;   /* The step number that athe character moves in a direction */    char    cinAli[HEIGHT][WIDTH], /* the Character of the Game*/            action;    /* Type of action that user enters */    /* Initialize Cin Ali */    initialize(currentPosition, &direction, cinAli);    /* Play game until user exits */    do    {        /* Display menu and read the order user gave*/        action=menu();        /* Do the action */        switch (action)        {            case '>': /* Move Right */                setStepNumber(&step); /* Read and set the step number */                turnRight(currentPosition, &direction, cinAli);                walk(step, direction, &currentPosition, cinAli);                break;            case '<': /* Move left */                setStepNumber(&step);                turnLeft(currentPosition, &direction, cinAli);                walk(step, direction, &currentPosition, cinAli);                break;            case '=': /* Turn Front */                turnFront(currentPosition, &direction, cinAli);                break;            case 'r': /* Turn Right */                turnRight(currentPosition, &direction, cinAli);                break;            case 'l': /* Turn Left */                turnLeft(currentPosition, &direction, cinAli);                break;            case 'x': /* Exit */                gameOver();                break;            case 'j':                setStepNumber(&step);                turnFront(currentPosition, &direction, cinAli);                jump(step, currentPosition, direction, cinAli);                break;            default:                error(incorrectInput); /* Incorrect input from the user */                break;        }        flushInputBuffer(); /* Clean the input buffer */    }while(action!='x');    return 0;}

/** makeBox ************************************************************** * Makes the robot move in a 2'x2' square * ***********************************************************************/void makeBox() {	driveAhead(BOX_WIDTH);	turnRight();	driveAhead(BOX_WIDTH);	turnRight();	driveAhead(BOX_WIDTH);	turnRight();	driveAhead(BOX_WIDTH);	turnRight();}

//Left/right sonar turns, curve and hard with compas turn ratiovoid sonarObsticalCheck(int inType){	zA=0;	nxtDisplayCenteredBigTextLine(1, "X: %d", xA);	nxtDisplayCenteredBigTextLine(3, "Y: %d", yA);	nxtDisplayCenteredBigTextLine(6, "Z: %d", zA);	switch ( inType ){		case 1:			if ( us_NewResult < 10 ){	    		turnRight();	    		wait1Msec(50);	    			if ( us_NewResult < 10 ){	    				turnRight();	    				wait1Msec(50);	    			} else {	    				turnLeft();	    				wait1Msec(50);	    			}	    	}	else if ( us_OldResult < 10 ) {	    		turnLeft();	    		wait1Msec(50);	    			if ( us_OldResult < 10 ) {	    				turnLeft();	    				wait1Msec(50);	    			}else{	    				turnRight();	    				wait1Msec(50);	    			}	    	}				break;		case 2:			if ( ( us_NewResult > 10 ) && ( us_OldResult < 10 ) ){					do{ motor[RIGHT] = 40; motor[LEFT] = -40; }while( zA <= 75 );	  	}	else if ( ( us_NewResult < 10 ) && ( us_OldResult > 10 ) ) {	  			do{ motor[RIGHT] = -40; motor[LEFT] = 40; }while( zA >= -75 );	  	} else {	    		int whatToDo = random(100);	    		if ( whatToDo > 50 ){						do{ motor[RIGHT] = 40; motor[LEFT] = -40; }while( zA <= 75 );	  			}else{		    		zA=0;		  			do{ motor[RIGHT] = -40; motor[LEFT] = 40; }while( zA >= -75 );	  			}	    }			break;		case 3:			int whatToDoNow = random(100);  		if ( whatToDoNow > 50 ){    		do{ motor[RIGHT] = 40; motor[LEFT] = -40; }while( zA <= 75 );			}else{    		do{ motor[RIGHT] = -40; motor[LEFT] = 40; }while( zA >= -75 );			}			break;		}	}

void kakitRed (){// variables	int armMin = 300;	int wallHeight = 1000;	int goalHeight = 1550;	int dead1 = 1000;	int dead2 = 2000;	int dead3 = 3000;	int pot = analogRead (8);	int encoder1 = 1000;	int encoder2 = 150;	int encoder3 = 2000;	int encoder4 = 75;	int encoder5 = 75;	int encoder6 = 75;	int encoder7 = 75;	int encoder8 = 75;	int encoder9 = 75;	// begin routine	driveForwardDead (); //ram big balls	intakeDead ();	delay (3000);	stopIntake();	driveBackDead (4000); // wall align to 90 deg	driveForward (encoder1);	turnRight (encoder2); // turn towards buckys	intakeDead ();	//line follow forward (encoder 4)	driveBackSlowDead (); // allign the bump	driveBackDead();  // over the bump	driveForwardSlowDead(); // alighn to bump	driveBackSlow(encoder4);	turnRight (encoder5);	driveBackSlow (encoder6) ; // go under the bridge	armUpDead (); // break the bridge	delay(500);	armDown (armMin);	driveBack (encoder7);	turnRight (encoder8);	armUp (goalHeight);	//line follow (encoder9);	outtake(8000); // score all three balls in the goal	stopAll ();}

task main(){    motor[lift] = 0;    //waitForStart();    initializeRobot();    int centerGoal;    moveForward(2.5);    wait10Msec(50);    readSensor(&irSeeker);    centerGoal = irSeeker.acDirection;    displayTextLine(1, "D:%4d", irSeeker.acDirection);    if(centerGoal >= 0 && centerGoal <= 3)    {        playSound(soundBeepBeep);        turnRight(90);        wait10Msec(50);        moveForward(1.5);        wait10Msec(50);    }    else if(centerGoal > 3 && centerGoal < 5)    {        playSound(soundDownwardTones);        moveBackward(1);        wait10Msec(50);        turnRight(70);        wait10Msec(50);        moveForward(2);        wait10Msec(50);    }    else if(centerGoal >= 5 && centerGoal <= 9)    {        playSound(soundFastUpwardTones);        moveBackward(2);        wait10Msec(50);        turnRight(90);        wait10Msec(50);        moveForward(1);        wait10Msec(50);    }    else    {        stopBot();    }}

void drawTheta(Servo *leftWheel, Servo *rightWheel, Servo *penServo){    ///// DRAW THE OUTER SHELL OF THE THETA /////  //Change DISTANCE in constants.h to change size of the Theta        forward(leftWheel, rightWheel, DISTANCE*.293);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws top right corner    forward(leftWheel, rightWheel, DISTANCE*.293);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws the right edge    forward(leftWheel, rightWheel, DISTANCE*.587);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws lower right corner    forward(leftWheel, rightWheel, DISTANCE*.293);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws bottom of Theta    forward(leftWheel, rightWheel, DISTANCE*.293);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws bottom left corner    forward(leftWheel, rightWheel, DISTANCE*.293);        turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN - 10);    forward(leftWheel, rightWheel, DISTANCE*.587); //Draws the left Theta edge          turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws top left corner    forward(leftWheel, rightWheel, DISTANCE*.293);           turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN);    ///// DRAW THE MIDDLE PART OF THE THETA /////  //First, position the robot.   penUp(leftWheel, rightWheel, penServo); //Raise pen up  turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES); // Turn right  forward(leftWheel, rightWheel, DISTANCE*.65);            // Go towards the center of circle    // Now, let's actually draw the "lightning bolt", or center.  turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES * 1.5); // Turn to get ready for drawing  penDown(leftWheel, rightWheel, penServo);  // Put the pen down.    // This part will actually draw the center.  forward(leftWheel, rightWheel, DISTANCE*.146);  turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES / 2);  forward(leftWheel, rightWheel, DISTANCE*.146);  turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES / 2);  forward(leftWheel, rightWheel, DISTANCE*.146);    // Stop the robot and lift the pen.  // Freeze for ten seconds.  penUp(leftWheel, rightWheel, penServo);  stopBot(leftWheel, rightWheel, DISTANCE);}

task main(){	StartTask(intakeStart);	while(SensorValue[bumperLeft]==0)	{	}	ClearTimer(T4);	moveSecondTierUp(127,450);	moveSecondTierDown(127,50);	intake = 1;	wait10Msec(50);	moveStraightDistance(127,200);	stopPid(0.6,0.3);	wait10Msec(10);  moveStraightDistance(30, 200);  stopPid(0.6,0.3);  wait10Msec(200);  intake = 0;	turnRight(100,250);	moveStraightDistance(100,100);	alignFoward(127);	wait10Msec(5);	stopDrive();	moveSharpRight(127,600);	moveStraightDistance(127,100);	stopPid(0.6,0.3);	moveFirstTierUp(127,1800);	moveFirstTierDown(127,50);	crossRamp(127,300,0);	moveStraightTime(-127, 500);	if (time1[T4] < 10000)	{		moveSharpRight(127,50);		moveStraightDistance(127,250);		stopPid(0.6,0.3);		pushBridge(127,800);		moveSecondTierUp(100,200);		moveStraightDistance(-127,100);		turnRight(127,250);		alignFoward(127);		moveStraightDistance(127,100);		stopPid(0.6,0.3);		moveStraightLight(127);		turnLeft(127,250);		moveStraightDistance(127,100);		stopPid(0.6,0.3);		stopLift();	}  StopTask(intakeStart);}

	task usercontrol()	{		// User control code here, inside the loop		while (true)		{			motor[DFR]=vexRT[Ch2];			motor[DBR]=vexRT[Ch2];			motor[DFL]=vexRT[Ch2];			motor[DBL]=vexRT[Ch2];			motor[middle]=vexRT[Ch1];			if(abs(vexRT[Ch4]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)			{				turnRight(127);			}			else			{				turnRight(0);			}				if(abs(vexRT[Ch3]) > tol)//set straight			straight = vexRT[Ch3];		else			straight = 0;		if(abs(vexRT[Ch1]) > tol) //set ptturn			ptturn = vexRT[Ch1];		else			ptturn = 0;		if(abs(vexRT[Ch4]) > tol) //set side			side = vexRT[Ch4];		else			side = 0;		if((abs(-ptturn + straight)<=127) && (abs(ptturn + straight)<=127))		{			driveRight(-ptturn/2 + straight*2/3);			driveLeft(ptturn/2 + straight*2/3);			motor[holo]= side;		}		else if(abs(-ptturn + straight) > 127)//(pttrn + straight)		{			driveRight(100);			driveLeft(-100);			motor[holo]= side;		}		else if(abs(-ptturn + straight) > 127)		{			driveRight(-100);			driveLeft(100);			motor[holo]= side;		}		}	}

void pickSort(int armNo,int sortNo){	printf("Arm %d picked %d from sort[%d]/n",armNo,sort[sortNo],sortNo);	arm[armNo]=sort[sortNo];	if(armNo==0)		turnLeft();	else turnRight();	pick(armNo);	if(armNo==0)		turnRight();	else turnLeft();	sort[sortNo]=-1;}

void loop() {    // all sensors are active low    // LOW = white    HIGH = black//  int goright=2;//  int gostraight=1;//  int gostation=0;//  int tickgiven = 10;    int pop;    int skip = 0;    while (1) {        RCmode();        if (xQueueReceive(directionQ, &pop, QDELAY)) {//        printf("nothing in queue!/n");            if (pop == 2) {                turnRight();                #if DEBUG                    puts("LF right");                #endif            }            if (pop == 1) {                if (skip == 1) {                    turnRight();                    #if DEBUG                        puts("LF turn right");                    #endif                    skip++;                }                else {                    straight();                    skip++;                    #if DEBUG                        puts("LF straight");                    #endif                }            }            if (pop == 0) {                station();                skip = 0;                #if DEBUG                    puts("LF station");                #endif                (xQueueSend(lineFollowertoSM, &skip, QDELAY));            }        }  //end if xQueueReceive    }  //end while}  //end loop()

task main(){  initializeRobot();  waitForStart(); // Wait for the beginning of autonomous phase.  moveForward((1400*9),100);  turnRight((1400*6),80);  moveForward((1400*16),-100);  turnLeft((1400*6),80);  moveForward((1400*2),100);  turnRight((1400*6),80);  moveForward((1400*0.5),-100);}

void autonomous(){	int encoder1 = digitalRead (1);	int limit2 = digitalRead (2);intakeDead();//turnRightDead();//delay (500);driveForwardDead();delay (1000);	stopIntake();	driveBackDead();	delay (2000);	turnRight(500);	outtakeDead (3000);if (encoder1 < 300 ) // bottom  is activated{	stopIntake();	driveBackDead();	delay (2000);	turnRight(500);	outtakeDead (1000);}else	stopIntake();	driveBackDead();	delay (2000);	stopAll(20000);//rushBlue ();//allLine (99999);// all jumper comands here:}

void Decide(void) {    int t = LEFT_THRESHOLD; // easier than typing it all out    // if we reached the end do a 180    if (reached == 1) {        forward();        msDelay(500);        turnRight();        msDelay(3000);        stop();        // we have turned now.        reached = 0;    }    // bbb    if (left_opto < t && middle_opto < t && right_opto < t) {        count++;        // first time dont count        if (count == 1) {            forward();        } else {            reached = 1;        }        return;    }        //www    else if (left_opto > t && middle_opto > t && right_opto > t) {        forward();    }        //wbw    else if (left_opto > t && middle_opto < t && right_opto > t) {        forward();    }        //bww    else if (left_opto < t && middle_opto > t && right_opto > t) {        turnLeft();    }        //wwb    else if (left_opto > t && middle_opto > t && right_opto < t) {        turnRight();    }    // No, we didnt reach the end    reached = 0;}

task main(){HTGYROstartCal(Gyro);motor[leftWheel] = 100;motor[rightWheel] = 100;wait10Msec(20);motor[leftWheel] = 0;motor[rightWheel] = 0;wait10Msec(100);while(turnRight(40)){	motor[leftWheel] = -100;	motor[rightWheel] = 100;}motor[leftWheel] = 0;motor[rightWheel] = 0;wait10Msec(100);while(SensorValue[ultraSonic] > 30){	motor[leftWheel] = 100;	motor[rightWheel] = 100;}	motor[leftWheel] = 0;	motor[rightWheel] = 0;}

task autonomous(){	//Move the robot forward for 1000 encoder counts	//at a speed of 50, then wait for half of a second//	intake();	StartTask(moveArm);	turnLeft(130,127);	moveForward(1450, 127);//	wait1Msec(500);	//Turn the robot to the right for 285 encoder counts	//at a speed of 25, then wait for half of a second//	turnRight(370, 25);	moveBackward();//	wait1Msec(500);	//Turn the robot to the left for 285 encoder counts	//at a speed of 25, then wait for half of a second	StartTask(downArm);	turnLeft(145, 127);//	wait1Msec(500);	moveForward(1800,127);	turnRight(214,127);	StartTask(moveArm);	moveForward(350,127);	motor[leftIntake] = 70;	motor[rightIntake] = 70;	wait1Msec(2000);	//AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.}

// Basic Left wall following codevoid wallFollowingL(){	int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;	while (1)	//for(i = 0; i < 250; i++)	{		get_front_ir_dists(&leftFrontIR, &rightFrontIR);		get_side_ir_dists(&leftSideIR, &rightSideIR);		distAhead = get_us_dist(); 				printf("i = %i/n", i);				if ( rightFrontIR < 30 )		{ turnRight (90.0, 5);} // super turn right				else if (leftFrontIR > 30 && leftSideIR > 25)		{ set_motors(SPEED-15, SPEED+50);} // super turn left				else if (leftFrontIR > 38)		{ set_motors(SPEED, 30);} // too far, turn left				else if ( leftFrontIR < 18 )		{ set_motors(SPEED*1.5, SPEED);}  // turn right				else		{set_motors(SPEED+5, SPEED+5); } // Straight	}}