#pragma config(Sensor, in6,    potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  frontTouch,     sensorTouch)
#pragma config(Sensor, dgtl8,  sonarCM,        sensorSONAR_cm)
#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           armMotor,      tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                              - Forward for Distance with Encoders -                                *|
|*                                      ROBOTC on VEX 2.0 CORTEX                                      *|
|*                                                                                                    *|
|*  This program instructs the robot to move forward for 5 rotations of the left shaft encoder.       *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the        *|
|*        "Squarebot" model, but may not be needed for all robot configurations.                      *|
|*    2)  Whichever encoder is being used for feedback should be cleared just before it starts        *|
|*        counting by using the "SensorValue(encoder) = 0;".  This helps ensure consistancy.          *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor   - Port 2    rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor   - Port 3    leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Digital - Port 1,2  rightEncoder        VEX Shaft Encoder     Right side                        *|
|*    Digital - Port 3,4  leftEncoder         VEX Shaft Encoder     Left side                         *|
\*----------------------------------------------------------------------------------------------------*/


//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++

void moveStraight (int encoderCount)

{
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	//First Forward Movement
	while(SensorValue[leftEncoder] < encoderCount)  // While less than 550 for forward to 2'
	{

		//...Move Forward
		if(SensorValue[leftEncoder]> SensorValue[rightEncoder])
		{
			motor[rightMotor] = 127;
			motor[leftMotor] = 127;
		}
		if(SensorValue[rightEncoder]> SensorValue[leftEncoder])
		{
			motor[rightMotor] = 127;
			motor[leftMotor] = 127;
		}
		if(SensorValue[leftEncoder]== SensorValue[rightEncoder])
		{
			motor[rightMotor] = 127;
			motor[leftMotor] = 127;
		}
	}
}

void turnLeft ()
{

	SensorValue[rightEncoder] = 0;
  SensorValue[leftEncoder] = 0;

  while(SensorValue[leftEncoder] > -280)  // Swing Turn to Left

  if(SensorValue[leftEncoder]> SensorValue[rightEncoder])
	{
		motor[rightMotor] = 63;
		motor[leftMotor] = -63;
  }
  if(SensorValue[rightEncoder]> SensorValue[leftEncoder])
  {
  	motor[rightMotor] = 63;
  	motor[leftMotor] = -63;
  }
  if(SensorValue[leftEncoder]== SensorValue[rightEncoder])
  {
  	motor[rightMotor] = 63;
  	motor[leftMotor] = -63;
  }
}

void turnRight ()
{

	SensorValue[rightEncoder] = 0;
  SensorValue[leftEncoder] = 0;

  while(SensorValue[leftEncoder] < 315)  // Swing Turn to Right

  if(SensorValue[leftEncoder]> SensorValue[rightEncoder])
	{
		motor[rightMotor] = -63;
		motor[leftMotor] = 63;
  }
  if(SensorValue[rightEncoder]> SensorValue[leftEncoder])
  {
  	motor[rightMotor] = -63;
  	motor[leftMotor] = 63;
  }
  if(SensorValue[leftEncoder]== SensorValue[rightEncoder])
  {
  	motor[rightMotor] = -63;
  	motor[leftMotor] = 63;
  }
}

	task main()
	{


		wait1Msec(500);  // .5 Second Delay

		moveStraight (2013);
		turnLeft ();
		moveStraight(300);
		turnLeft ();





		SensorValue[rightEncoder] = 0;
		SensorValue[leftEncoder] = 0;



	}