text: #pragma config(Sensor, dgtl9,  enc1,           sensorQuadEncoder)
  #pragma config(Sensor, dgtl11, enc2,           sensorQuadEncoder)
  #pragma config(Motor,  port1,           hMotor,        tmotorVex393_HBridge, openLoop)
  #pragma config(Motor,  port2,           l_frontDrive,  tmotorVex393_MC29, openLoop, driveLeft, encoderPort, None)
  #pragma config(Motor,  port3,           r_frontDrive,  tmotorVex393_MC29, openLoop, driveRight, encoderPort, None)
  #pragma config(Motor,  port4,           rLift,         tmotorVex393_MC29, openLoop)
  #pragma config(Motor,  port5,           lLift,         tmotorVex393_MC29, openLoop)
  #pragma config(Motor,  port6,           rClaw,         tmotorVex393_MC29, openLoop)
  #pragma config(Motor,  port7,           lClaw,         tmotorVex393_MC29, openLoop)
  #pragma config(Motor,  port8,           l_backDrive,   tmotorVex393_MC29, openLoop, driveLeft, encoderPort, None)
  #pragma config(Motor,  port9,           r_backDrive,   tmotorVex393_MC29, openLoop, driveRight)
  //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  
  /*---------------------------------------------------------------------------*/
  /*                                                                           */
  /*        Description: Competition template for VEX EDR                      */
  /*                                                                           */
  /*---------------------------------------------------------------------------*/
  
  // This code is for the VEX cortex platform
  #pragma platform(VEX2)
  
  
  
  // Select Download method as "competition"
  #pragma competitionControl(Competition)
  #pragma autonomousDuration(15)
  #pragma userControlDuration(145)
  
  //Main competition background code...do not modify!
  #include "Vex_Competition_Includes.c"
  
  /*---------------------------------------------------------------------------*/
  /*                          Pre-Autonomous Functions                         */
  /*                                                                           */
  /*  You may want to perform some actions before the competition starts.      */
  /*  Do them in the following function.  You must return from this function   */
  /*  or the autonomous and usercontrol tasks will not be started.  This       */
  /*  function is only called once after the cortex has been powered on and    */
  /*  not every time that the robot is disabled.                               */
  /*---------------------------------------------------------------------------*/
  
  void pre_auton()
  {
    // Set bStopTasksBetweenModes to false if you want to keep user created tasks
    // running between Autonomous and Driver controlled modes. You will need to
    // manage all user created tasks if set to false.
    bStopTasksBetweenModes = true;
  
  	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
  	// used by the competition include file, for example, you might want
  	// to display your team name on the LCD in this function.
  	//bDisplayCompetitionStatusOnLcd = false;
  
    // 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.   */
  /*---------------------------------------------------------------------------*/
  
  #define PID_INTEGRAL_LIMIT  50
  #define PID_DRIVE_MAX       127
  #define PID_DRIVE_MIN     (-127)
  
  
  bool antiGravToggle = true;
  
  void antiGrav()
  {
  		if(antiGravToggle)
  		{
  			motor[rLift] = -20;
  			motor[lLift] = -20;
  		}
  
  		else
  		{
  			motor[rLift] = 127;
  			motor[lLift] = 127;
  		}
  }
  
  void hDrive(int h)
  {
  	motor[hMotor] = h;
  }
  
  static float pidRequestedValueLeft, pidRequestedValueRight;
  int hMotor = 0;//use to strafe
  static float Kp = .55;
  static float Ki = 0;
  static float Kd = 0;
  static float leftDriveScale = .55; //Multiply faster motor by this so speed is equal with slower motor
  static float rightDriveScale = 1;
  static bool pidActive = true;
  
  
  task pidController()
  {
  	//Initialize variables
  	float  pidSensorCurrentValueLeft, pidSensorCurrentValueRight;
  	float  pidErrorLeft, pidErrorRight;
  	float  pidLastErrorLeft, pidLastErrorRight;
  	float  pidIntegralLeft, pidIntegralRight;
  	float  pidDerivativeLeft, pidDerivativeRight;
  	float  pidDriveLeft, pidDriveRight;
  
  	//Clear sensor values
  	SensorValue[enc1] = 0;
  	SensorValue[enc2] = 0;
  
  	pidLastErrorLeft = 0;
  	pidLastErrorRight = 0;
  	pidIntegralLeft = 0;
  	pidIntegralRight = 0;
  
  	while(true)
  	{
  		antiGrav(); //Keeps lift stationary
  		hDrive(hMotor);
  		//If the pid is set to active
  		if(pidActive)
  		{
  			//Calculate proportional portion
  			pidSensorCurrentValueLeft = SensorValue[enc1];
  			pidSensorCurrentValueRight = SensorValue[enc2];
  
  			pidErrorLeft = pidSensorCurrentValueLeft - pidRequestedValueLeft;
  			pidErrorRight = pidSensorCurrentValueRight - pidRequestedValueRight;
  			// Calculate integral portion
  			// Add the error to the integral if it is outside the controllable range of values
  			if(abs(pidErrorLeft) < PID_INTEGRAL_LIMIT)
  				pidIntegralLeft = pidIntegralLeft + pidErrorLeft;
  			else
  				pidIntegralLeft = 0;
  
  			if(abs(pidErrorRight) < PID_INTEGRAL_LIMIT)
  				pidIntegralRight = pidIntegralRight + pidErrorRight;
  			else
  				pidIntegralRight = 0;
  
  			// Calculate derivitave portion
  			pidDerivativeLeft = pidErrorLeft - pidLastErrorLeft;
  			pidDerivativeRight = pidErrorRight - pidLastErrorRight;
  
  			pidLastErrorLeft  = pidErrorLeft;
  			pidLastErrorRight = pidErrorRight;
  
  			// calculate drive
  			pidDriveLeft = (Kp * pidErrorLeft) + (Ki * pidIntegralLeft) + (Kd * pidDerivativeLeft);
  			pidDriveRight = (Kp * pidErrorRight) + (Ki * pidIntegralRight) + (Kd * pidDerivativeRight);
  
  			// limit drive
  			if( pidDriveLeft > PID_DRIVE_MAX )
  				pidDriveLeft = PID_DRIVE_MAX;
  			if( pidDriveLeft < PID_DRIVE_MIN )
  				pidDriveLeft = PID_DRIVE_MIN;
  
  			if( pidDriveRight > PID_DRIVE_MAX )
  				pidDriveRight = PID_DRIVE_MAX;
  			if( pidDriveRight < PID_DRIVE_MIN )
  				pidDriveRight = PID_DRIVE_MIN;
  
  
  			// DRIVE BOI DRIVE
  			motor[l_frontDrive] = (int)((float)pidDriveLeft * (-1.0)*leftDriveScale);
  			motor[l_backDrive] = (int)((float)pidDriveLeft * (-1.0)*leftDriveScale);
  			motor[r_frontDrive] = (int)((float)pidDriveRight * rightDriveScale);
  			motor[r_backDrive] = (int)((float)pidDriveRight * rightDriveScale);
  		}
  		else // delete the emails, i mean variables
  		{
  			pidSensorCurrentValueLeft = 0;
  			pidSensorCurrentValueRight = 0;
  			pidErrorLeft = 0;
  			pidErrorRight = 0;
  			pidLastErrorLeft = 0;
  			pidLastErrorRight = 0;
  			pidIntegralLeft = 0;
  			pidIntegralRight = 0;
  			pidDerivativeLeft = 0;
  			pidIntegralRight = 0;
  			pidDriveLeft= 0;
  			pidDriveRight = 0;
  			motor[l_frontDrive] = 0;
  			motor[l_backDrive] = 0;
  			motor[r_frontDrive] = 0;
  			motor[r_backDrive] = 0;
  		}
  
  	}
  }
  
  
  void drive(int anglesLeft, int anglesRight)
  {
  	SensorValue[leftEnc] = 0;
  	SensorValue[rightEnc] = 0;
  
  	pidRequestedValueLeft = anglesLeft;
  	pidRequestedValueRight = anglesRight;
  
  }
  
  
  task autonomous()
  {
  	// 1000 angles equals 2 ft movement (1 square)
  	pidRequestedValueLeft = 0;
  	pidRequestedValueRight = 0;
  
  	motor[leftClaw] = -127;
  	motor[rightClaw] = -127;
  	wait1Msec(500);
  	motor[leftClaw] = 0;
  	motor[rightClaw] = 0;
  
  	motor[leftTopLift] = -127;
  	motor[leftBottomLift] = -127;
  	motor[rightTopLift] = -127;
  	motor[rightBottomLift] = -127;
  
  	wait1Msec(800);
  	// start the PID task
  	startTask(pidController);
  	drive(2200,2200);
  	hMotor = -127;
  	wait1Msec(200);
  	hMotor = 0;
  	/*Left side outward
  	wait1Msec(3000);
  	drive(-600,-600);
  
  	wait1Msec(1000);
  	hMotor = -127;
  	wait1Msec(800)
  	hMotor = 0;
  	drive(600, 600);
  	*/
  
  	//Left side outward aka FENCE RAPE
  	wait1Msec(2800);
  	drive(-1200,-1200);
  	wait1Msec(1000);
  	antiGravToggle = false;
  	wait1Msec(300);
  	antiGravToggle = true;
  	drive(200,0);
  	hMotor = 127;
  	wait1Msec(1500);
  	hMotor = 0;
  
  	drive(1200,1200);
  	wait1Msec(1500);
  	drive(-600,-600);
  
  }
  /* Tasks so that we could do a step function hopefully :) */
  task liftUp()
  {
  	motor[lLift] = 40;
  	motor[rLift] = 40;
    wait1Msec(500);
  	motor[lLift] = 0;
  	motor[rLift] = 0;
  }
  
  task liftDown()
  {
  	motor[lLift] = -40;
  	motor[rLift] = -40;
    wait1Msec(500);
  	motor[lLift] = 0;
  	motor[rLift] = 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()
  {
    // User control code here, inside the loop
  	while (true)
  	{
  		motor[l_frontDrive]  = (vexRT[Ch1] - vexRT[Ch3]);
  		motor[r_frontDrive]  = (vexRT[Ch1] + vexRT[Ch3]);
  		motor[l_backDrive]   = (vexRT[Ch1] + vexRT[Ch3]);
  		motor[r_backDrive]   = (vexRT[Ch1] - vexRT[Ch3]);
  		motor[hMotor]        = (vexRT[Ch4] + vexRT[Ch2]);
  
  		if (vexRT[Btn5U] == 1)
  		{
  			startTask(liftUp);
  		}
  		else if (vexRT[Btn5D] == 1)
  		{
  			startTask(liftDown);
  		}
  
  		if (vexRT[Btn6U] == 1)
  		{
  			motor[rClaw] = 127;
  			motor[lClaw] = 127;
  		}
  		else if (vexRT[Btn6D] == 1)
  		{
  			motor[rClaw] = -127;
  			motor[lClaw] = -127;
  		}
  		else
  		{
  			motor[rClaw] = 10;
  			motor[lClaw] = 10;
  		}
  
    	wait1Msec(20);
      // This is the main execution loop for the user control program.
      // Each time through the loop your program should update motor + servo
      // values based on feedback from the joysticks.
  
      // ........................................................................
      // Insert user code here. This is where you use the joystick values to
      // update your motors, etc.
      // ........................................................................
  
      // Remove this function call once you have "real" code.
  
    }
  }
  