#pragma config(Sensor, in2,    ,               sensorPotentiometer)
#pragma config(Sensor, dgtl1,  spatula,        sensorDigitalOut)
#pragma config(Sensor, dgtl2,  descore,        sensorDigitalOut)
#pragma config(Motor,  port2,           frontRight,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           backRight,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,            ,             tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port5,            ,             tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           frontLeft,     tmotorVex393, openLoop)
#pragma config(Motor,  port7,           backLeft,         tmotorVex393, openLoop)
#pragma config(Motor,  port8,            ,             tmotorVex393, openLoop)
#pragma config(Motor,  port9,            ,             tmotorVex269, openLoop)
#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!

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()
{
  // .....................................................................................
  // Insert user code here.
  // .....................................................................................

   AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 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)
   {
     int X2 = 0, Y1 = 0, X1 = 0, threshold = 5;
   bool spatulafired = false;
   int desiredvalue = 3045;
   bool manual = true;
   while(1 == 1)
   {

      //Create "deadzone" for Y1/Ch3
      if(abs(vexRT[Ch3]) > threshold)
         if (abs(vexRT[Ch3]) < 110){
         Y1 = (vexRT[Ch3]/2);
         } else {
         Y1 = vexRT[Ch3];
      }
      else
         Y1 = 0;
      //Create "deadzone" for X1/Ch4
      if(abs(vexRT[Ch4]) > threshold)
         if (abs(vexRT[Ch4]) < 110){
         X1 = (vexRT[Ch4]/2);
         } else {
         X1 = vexRT[Ch4];
      }
      else
         X1 = 0;
      //Create "deadzone" for X2/Ch1
      if(abs(vexRT[Ch1]) > threshold)
         if (abs(vexRT[Ch1]) < 110){
         X2 = (vexRT[Ch1]/2);
         } else {
         X2 = vexRT[Ch1];
      }
      else
         X2 = 0;

      //Perform calculations and set motors accordingly.
      motor[frontRight] = Y1 - X2 - X1;
      motor[backRight] =  Y1 - X2 + X1;
      motor[frontLeft] = Y1 + X2 + X1;
      motor[backLeft] =  Y1 + X2 - X1;
   }
   //Controls for spatula pneumatics.
   if(vexRT[Btn8D] == 1){
      if(spatulafired == true){
         spatulafired = false;
         } else {
         spatulafired = true;
      }
   }
   if(spatulafired == true){
      SensorValue[spatula] = 1;
      }else{
      SensorValue[spatula] = 0;
   }
   
   if(vexRT[Btn6U] == 1){
      manual = true;
      motor[port4] = 90;
      motor[port5] = 90;
      motor[port8] = 90;
      motor[port9] = 90;
   } else if(vexRT[Btn6D] == 1){
      manual = true;
      motor[port4] = -90;
      motor[port5] = -90;
      motor[port8] = -90;
      motor[port9] = -90;
   } else {
      if(manual == true){
         motor[port4] = 0;
         motor[port5] = 0;
         motor[port8] = 0;
         motor[port9] = 0;
      }
   }
   
   if (vexRT[Btn7D] == 1){
      manual = false;
      desiredvalue = 3045;
   }else if(vexRT[Btn7L] == 1){
      manual = false;
      desiredvalue = 2504;
   }else if(vexRT[Btn7U] == 1){
      manual = false;
      desiredvalue = 1828;
   }
   
   if (manual == false){
      if(abs(SensorValue[in2] - desiredvalue) > 15){
         if (SensorValue[in2] > desiredvalue){
            motor[port4] = 127;
            motor[port5] = 127;
            motor[port8] = 127;
            motor[port9] = 127;
         } else {
            motor[port4] = -127;
            motor[port5] = -127;
            motor[port8] = -127;
            motor[port9] = -127;
         }
      } else if(abs(SensorValue[in2] - desiredvalue) <= 15 && abs(SensorValue[in2] - desiredvalue) > 2){
         if (SensorValue[in2] > desiredvalue){
            motor[port4] = 63;
            motor[port5] = 63;
            motor[port8] = 63;
            motor[port9] = 63;
         } else {
            motor[port4] = -63;
            motor[port5] = -63;
            motor[port8] = -63;
            motor[port9] = -63;
         }
      } else {
         motor[port4] = 0;
         motor[port5] = 0;
         motor[port8] = 0;
         motor[port9] = 0;
      }
   }
   }
}

 motor[frontRight] = Y1 - X2 - X1;
      motor[backRight] =  Y1 - X2 + X1;
      motor[frontLeft] = Y1 + X2 + X1;
      motor[backLeft] =  Y1 + X2 - X1;
   **}**