blkautoback 3 enctestIRGYRO.c

#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     touch,          sensorTouch)
#pragma config(Sensor, S4,     IR2,            sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          vacuumA,       tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,          vacuumB,       tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     flag,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C1_2,     lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     leftMotor,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     rightMotor,    tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

//TJoystick joystick;

task main()
{
  waitForStart();		                // Wait for the field to enter autonomous mode

        // Sequential sub-routines for each individual move
        // Various encoder revolutions to fine tune moves.  34" = 4000 encoder ticks


  				// Raise Scissors higher than basket
	    	  int c = 0;
  		    while(c < 9)
		     	{
		      motor[lift] = 80;         // Motor Lift is run at a 80 power level.
		      wait1Msec(1000);
          c++;
    		  }
        	motor[lift] = 0;

//  			StopTask(displayDiagnostics);
//				eraseDisplay();


				// Move Forwared to Beacon
        nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        nMotorEncoder[rightMotor] = 0;
        wait1Msec(1200);							// Delay before next command

				while (SensorValue[S4] < 5)
				{
					motor[leftMotor] = 70; //turn both motors on at 50 percent power
	    		motor[rightMotor] = 70;
	    	}
				motor [leftMotor] = 0;
	    	motor [rightMotor] = 0;
				wait1Msec(100);
				int total = ((nMotorEncoder[leftMotor]) + 700);
//				nxtDisplayTextLine(1,"D:%i", nMotorEncoder[leftMotor]);
//				wait1Msec(100);
//				nxtDisplayTextLine(1,"D:%i", total);
//        wait1Msec(100);							// Delay before next command
//				while (true)
//					{
//						nxtDisplayTextLine(0, "DEnc=%d", nMotorEncoder[leftMotor]);
//						nxtDisplayTextLine(1, "EEnc=%d", nMotorEncoder[rightMotor];
//						wait1Msec(100);
//					}



				//Move slightly to align with center of basket
				if (nMotorEncoder[leftMotor] > -4000)
				{
				 	nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
				 	wait1Msec(200);								// Delay before next command
	     		while (nMotorEncoder[leftMotor] > -200)
	       	{
		    		motor[leftMotor] = 50;
	  	  		motor[rightMotor] = 50;
					}
	 			}
	  		else
	  		{
		 			nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
		 			wait1Msec(200);								// Delay before next command
     			while (nMotorEncoder[leftMotor] < 200)
	       	{
	    			motor[leftMotor] = -50;
	    			motor[rightMotor] = -50;
	    		}
 		    }
				motor[leftMotor] = 0;
				motor[rightMotor] = 0;


	      // Right Turn 90 degrees
        nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

        while (nMotorEncoder[leftMotor] > -1700) //while the encoder wheel turns one revolution
        {
	        motor[leftMotor] = -50; //turn both motors on at 60 percent power
	        motor[rightMotor] = 50;
        }
        motor[leftMotor] = 0; //turn both motors off
        motor[rightMotor] = 0;



        // Move Forward to basket
				nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[leftMotor] > -800) //while the encoder wheel turns one revolution//
	        {
	    		motor[leftMotor] = 50; //turn both motors on at 30 percent power
	    		motor[rightMotor] = 50;
	  			}

			  motor[leftMotor] = 0; //turn both motors off
 				motor[rightMotor] = 0;



        // Drop block in basket
       	int e = 0;
	      while(e < 1)
      	{
		      motor[vacuumA] = -100;    // Chute Motors expel block
		      motor[vacuumB] = -100;
		      wait1Msec(1000);
          e++;
        }
				motor[vacuumA] = 0;
				motor[vacuumB] = 0;



				// Move Backward away from basket
				nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[leftMotor] < 600) //while the encoder wheel turns one revolution//
	        {
	    		motor[leftMotor] = -50; //turn both motors on at 30 percent power
	    		motor[rightMotor] = -50;
	  			}

			  motor[leftMotor] = 0; //turn both motors off
 				motor[rightMotor] = 0;

        wait1Msec(200);             // Delay before starting next command


 				// Lower Scissors halfway to maintain stability while moving
        int g = 0;
	      while(g < 3)
      	{
		      motor[lift] = -80;        // Motor Lift is run at a 80 power level.
		      wait1Msec(1000);
          g++;
        }
        motor[lift] = 0;

        wait1Msec(100);							// Delay before next command


      // Right Turn 90 degrees
        nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

        while (nMotorEncoder[leftMotor] > -1700) //while the encoder wheel turns one revolution
        {
	        motor[leftMotor] = -50; //turn both motors on at 60 percent power
	        motor[rightMotor] = 50;
        }
        motor[leftMotor] = 0; //turn both motors off
        motor[rightMotor] = 0;



        // Move Forward to wall
				nMotorEncoder[leftMotor] = 0; //clear the TETRIX encoders in left motor only
        wait1Msec(200);							// Delay before next command

				while (nMotorEncoder[leftMotor] > total) // while leftmotor is still running (hasn't reached target yet):
				{
					motor[leftMotor] = 80; //turn both motors on at 50 percent power
					motor[rightMotor] = 80;
				}
				motor[leftMotor] = 0; //turn both motors off
				motor[rightMotor] = 0;



				// Left Turn 45 degrees
        nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

        while (nMotorEncoder[leftMotor] < 900) //while the encoder wheel turns one revolution
        {
        motor[leftMotor] = 50; //turn both motors on at 60 percent power
        motor[rightMotor] = -50;
        }
        motor[leftMotor] = 0; //turn both motors off
        motor[rightMotor] = 0;



        // Move Forward in front of RAMP
				nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[leftMotor] > -6100) //while the encoder wheel turns one revolution//
	        {
	    		motor[leftMotor] = 80; //turn both motors on at 30 percent power
	    		motor[rightMotor] = 80;
	  			}

			  motor[leftMotor] = 0; //turn both motors off
 				motor[rightMotor] = 0;



        // Right Turn 45 degrees
        nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

        while (nMotorEncoder[leftMotor] > -1500) //while the encoder wheel turns one revolution
        {
        motor[leftMotor] = -60; //turn both motors on at 60 percent power
        motor[rightMotor] = 60;
        }
        motor[leftMotor] = 0; //turn both motors off
        motor[rightMotor] = 0;



        // Move Backward onto RAMP
				nMotorEncoder[leftMotor] = 0;  //clear the TETRIX encoders in motors left and right
        wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[leftMotor] < 6000) //while the encoder wheel turns one revolution//
	        {
	    		motor[leftMotor] = -80; //turn both motors on at 30 percent power
	    		motor[rightMotor] = -80;
	  			}

			  motor[leftMotor] = 0; //turn both motors off
 				motor[rightMotor] = 0;

 }