#pragma config(Sensor, in1,    rightside,           sensorLineFollower)
#pragma config(Sensor, in2,    middle,              sensorLineFollower)
#pragma config(Sensor, in3,    leftside,            sensorLineFollower)
#pragma config(Sensor, dgtl1,  Input,               sensorSONAR_inch)
#pragma config(Motor,  port2,           rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port5,           Armmotor,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           Grippermotor,  tmotorNormal, openLoop, reversed)
#pragma platform(VEX)

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

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

void pre_auton()
{
   // All activities that occur before the competition starts
   // Example: clearing encoders, setting servo positions, ...
}

task autonomous()
{
  while (true)
  {
  motor[leftMotor]  = 63;
  motor[rightMotor] = 63;
} //   AuonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}


task usercontrol()
{
   // User control code here, inside the loop

   while (true)
   {
     // Button Control:
  //ArmMotor
 if(vexRT[Btn5U] == 1)      //If button 5U is pressed:
  {
    motor[Armmotor] = 64;         //run Armmotor at quarter speed (i.e. lift arm)
  }
  else if(vexRT[Btn5D] == 1) //If button 5D is pressed:
  {
    motor[Armmotor] = -32;        //run Armmotor at quarter speed reversed (i.e. lower arm)
  }
  else if(vexRT[Btn5D] == 0)
  {
    motor[Armmotor] = 16;
  }
   else if(vexRT[Btn5U] == 0)
  {
    motor[Armmotor] = 0;
  }

  //Grippermotor
   if(vexRT[Btn6U] == 1)      //If button 6U is pressed:
  {
    motor[Grippermotor] = 64;         //run Grippermotor  at quarter speed (i.e. lift arm)
  }
  else if(vexRT[Btn6D] == 1) //If button 6D is pressed:
  {
    motor[Grippermotor] = -64;        //run Grippermotor at quarter speed reversed (i.e. lower arm)
  }
  else if(vexRT[Btn6D] == 0)
  {
    motor[Grippermotor] = 0;
  }
   else if(vexRT[Btn6U] == 0)
  {
    motor[Grippermotor] = 0;
  }
  // Wheel Motors

    motor[leftMotor]  = vexRT[Ch3];   // Left Joystick Y value
    motor[rightMotor] = vexRT[Ch2];   // Right Joystick Y value
   }
}

#pragma config(Sensor, in1,    lineFollowerRIGHT,   sensorLineFollower)
#pragma config(Sensor, in2,    lineFollowerCENTER,  sensorLineFollower)
#pragma config(Sensor, in3,    lineFollowerLEFT,    sensorLineFollower)
#pragma config(Motor,  port2,           rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                                    - Triple Sensor Line Tracking -                                   *|
|*                                      ROBOTC on VEX 2.0 CORTEX                                      *|
|*                                                                                                    *|
|*  This program uses 3 VEX Line Follower Sensors to track a black line on a light(er) surface.       *|
|*  There is a two second pause at the beginning of the program.                                      *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the        *|
|*        "Squarebot" mode, but may not be needed for all robot configurations.                       *|
|*    2)  Lighting conditions, line darkness, and surface lightness change from place to place,       *|
|*        so the value of 'threshold' may need to be changed to better suit your environment.         *|
|*                                                                                                    *|
|*    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                   *|
|*    Analog - Port 1     lineFollowerRIGHT   VEX Light Sensor      Front-right, facing down          *|
|*    Analog - Port 2     lineFollowerCENTER  VEX Light Sensor      Front-center, facing down         *|
|*    Analog - Port 3     lineFollowerLEFT    VEX Light Sensor      Front-left, facing down           *|
\*-----------------------------------------------------------------------------------------------4246-*/


//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
 // wait1Msec(2000);          // The program waits for 2000 milliseconds before continuing.

  int threshold = 505;      /* found by taking a reading on both DARK and LIGHT    */
                            /* surfaces, adding them together, then dividing by 2. */
  while(true)
  {
    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -\
    displayLCDCenteredString(0, "LEFT  CNTR  RGHT");        //  Display   |
    displayLCDPos(1,0);                                     //  Sensor    |
    displayNextLCDNumber(SensorValue(lineFollowerLEFT));    //  Readings  |
    displayLCDPos(1,6);                                     //  to LCD.   |
    displayNextLCDNumber(SensorValue(lineFollowerCENTER));  //            |
    displayLCDPos(1,12);                                    //  L  C  R   |
    displayNextLCDNumber(SensorValue(lineFollowerRIGHT));   //  x  x  x   |
    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -/

    // RIGHT sensor sees dark:
    //  if(SensorValue(lineFollowerLEFT & lineFollowerCENTER & lineFollowerRIGHT) > threshold)
   //{
      // stop:
   //  motor[leftMotor]  = 0;
   //  motor[rightMotor] = 0;
   //}
   if(SensorValue(lineFollowerRIGHT) > threshold)
    {
      // counter-steer right:
      motor[leftMotor]  = 63;
      motor[rightMotor] = 0;
    }
    // CENTER sensor sees dark:
    if(SensorValue(lineFollowerCENTER) > threshold)
    {
      // go straight
      motor[leftMotor]  = 63;
      motor[rightMotor] = 63;
    }
    // LEFT sensor sees dark:
    if(SensorValue(lineFollowerLEFT) > threshold)
    {
      // counter-steer left:
      motor[leftMotor]  = 0;
      motor[rightMotor] = 63;
    }

  }
}