#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl11, sonar,          sensorSONAR_inch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           BackLeft,      tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port2,           FrontLeft,     tmotorVex393, openLoop)
#pragma config(Motor,  port3,           ClawPivot,     tmotorVex393, openLoop)
#pragma config(Motor,  port4,           LeftLift,      tmotorVex393, openLoop)
#pragma config(Motor,  port5,           RightLift,     tmotorVex393, openLoop)
#pragma config(Motor,  port8,           Claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port9,           FrontRight,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          BackRight,     tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	// if sonar is less than 10 do forward program
if (SensorValue[sonar] <= 10)
{
	{
	//Lift the lift
	motor[LeftLift] = 127;
	motor[RightLift] = 127;
	}
	wait1Msec(400);
	{
		//stop the lift
	motor[LeftLift] = 0;
	motor[RightLift] = 0;
	}
	wait1Msec(50);
	{
	//Move left to shake the cube off
		motor[FrontLeft] = -127;
		motor[BackLeft] = 127;
		motor[BackRight] = -127;
		motor[FrontRight]	= 127;
	}
	wait1Msec(600);
	{
		//stops motors
		motor[FrontLeft] = 0;
		motor[BackLeft]	= 0;
		motor[FrontRight] = 0;
		motor[BackRight]	= 0;
	}
	//Clear encoder
		nMotorEncoder[BackLeft] = 0;

		//Move forward past the cube on the left
	while(nMotorEncoder[BackLeft] < 750)
	{
	motor[FrontLeft] = 127;
		motor[BackLeft] = 127;
		motor[BackRight] = 127;
		motor[FrontRight]	= 127;
	}
	//Clear encoder
		nMotorEncoder[BackLeft] = 0;

		while(nMotorEncoder[BackLeft] < 1800)
	{
		//Move left past the cube
		motor[FrontLeft] = -127;
		motor[BackLeft] = 127;
		motor[BackRight] = -127;
		motor[FrontRight]	= 127;
	}

//Clear encoder
		nMotorEncoder[BackLeft] = 0;

	while(nMotorEncoder[BackLeft] < 1800)
	{
	//back up to the left of the cube
		motor[FrontLeft] = -127;
		motor[BackLeft] = -127;
		motor[BackRight] = -127;
		motor[FrontRight]	= -127;
	}

	//Clear encoder
		nMotorEncoder[BackLeft] = 0;

	while(nMotorEncoder[BackLeft] < -1600)
{
//move right to push block
motor[FrontLeft] = 127;
		motor[BackLeft] = -127;
		motor[BackRight] = 127;
		motor[FrontRight]	= -127;
	}

	}

//if sonar is more than 11 do backward program
else if (SensorValue[sonar] >= 11)
{
	{
	//Lift the lift
	motor[LeftLift] = 127;
	motor[RightLift] = 127;
	}
	wait1Msec(400);
	{
		//stop the lift
	motor[LeftLift] = 0;
	motor[RightLift] = 0;
	}
}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// Mechanum Wheels

	while (true)
	{
	  motor[FrontRight] = vexRT[Ch3]- vexRT[Ch4]- vexRT[Ch1];
    motor[BackRight] =  vexRT[Ch3]- vexRT[Ch4]+ vexRT[Ch1];
    motor[FrontLeft] = vexRT[Ch3]+ vexRT[Ch4]+ vexRT[Ch1];
    motor[BackLeft] =  vexRT[Ch3]+ vexRT[Ch4]- vexRT[Ch1];



  //Controls the claw pivot

if(vexRT[Btn8R] == 1)
    {
      motor[ClawPivot] = 90;

    }
    else if(vexRT[Btn8D] == 1)
    {
      motor[ClawPivot] = -90;

    }
    else
    {
      motor[ClawPivot] = 0;

    }
//Lift
    if(vexRT[Btn5U] == 1)
    {
      motor[LeftLift] = 70;
      motor[RightLift] = 70;

    }
    else if(vexRT[Btn5D] == 1)
    {
      motor[LeftLift] = -70;
      motor[RightLift] = -70;

    }
    else
    {
      motor[LeftLift] = 0;
      motor[RightLift] = 0;

    }


    //Claw Control
   if(vexRT[Btn6U] == 1)
    {
      motor[Claw] = 80;

    }
    else if(vexRT[Btn6D] == 1)
    {
      motor[Claw] = -80;

    }
    else
    {
      motor[Claw] = 0;

    }

    }


}