#pragma config(Motor,  port1,           rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           leftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port5,           motorA,        tmotorNormal, openLoop)
#pragma config(Motor,  port6,           motorB,        tmotorNormal, openLoop)
#pragma config(Motor,  port7,           servoA,        tmotorServo, openLoop)
#pragma config(Motor,  port8,           servoDP,       tmotorServo, openLoop)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
  int joy_x;            //hold values from channel 1 and 2 for driving
  int joy_y;           
  int threshold = 20;   // helps to eliminate 'noise' from a joystick that isn't perfectly at (0,0)
  int btn_5;          // will hold the value of buttons to determine if channel 3 will control the motors
  int btn_6;
  int joy_3;            // will hold the value of channel 3 for controlling motors
 // bVEXNETActive = false;
  //kNumbOfVexRFJoysticks = 1;
 //bIfiAutonomousMode = false;
  while(1==1)
  {
    joy_x = vexRT[Ch1];   // This is the RIGHT analog stick.  For LEFT, change 'Ch1' to 'Ch4'.
    joy_y = vexRT[Ch2];   // This is the RIGHT analog stick.  For LEFT, change 'Ch2' to 'Ch3'.


    // Forward, backward, and swing turns:
    if(abs(joy_y) > threshold)
    {
      motor[leftMotor]  = (joy_y + joy_x);
      motor[rightMotor] = (joy_y - joy_x);
    }
    // Turning in place:
   else if((abs(joy_x) > threshold) && (abs(joy_y) < threshold))
    {
     motor[leftMotor]  = joy_x;
      motor[rightMotor] = (-1 * joy_x);
    }
    // Standing still:
    else
    {
      motor[leftMotor]  = 0;
      motor[rightMotor] = 0;
    }
    // Assign control values for joystick for use in controlling 4 other motors with joystick 3
    joy_3 = vexRT[Ch3];
    btn_5 = vexRT[Ch5];
    btn_6 = vexRT[Ch6];

    //control motor3
    if (btn_6 > 0)
    {
    motor[motorA] = joy_3;
    }
    else
    {
    motor[motorA] = 0;
    }

    //control motor4
    if (btn_6 < 0)
    {
    motor[motorB] = joy_3;
    }
    else
    {
    motor[motorB] = 0;
    }

    //control servo5
    if (btn_5 > 0)
    {
    motor[servoA] = joy_3;
    }

    //control servoDP
    if (btn_5 < 1)
    {
    motor[servoDP] = joy_3;
    }

  }
}