#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorRightLift, tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeftLift, tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorLeftDrive, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorRightDrive, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorCentralLift, tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    servobb1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servobb2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^|
//                                                     |
//                 Teleop Code                         |
//    //             (C) 2011 Team: 3983                     |
//                                                     |
//^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^_^|

#include "JoystickDriver.c"

task main()
{

int elevator = 0;
int elevatorspeed = 50;
int servod = 100;
int servou = 255;
int eactive = 0;
int sactive = 0;
int qactive = 0;
int servoq = 225;
int servoqd = 130;
float dtx = 0;
int dtactive = 0;
float dtmin = 0.5;
float dtmax = 1;
int aactive = 0;
int as = 0;
int amax = 100;
int amin = 60;
int flips = 40;

waitForStart();

while (true)
  {
  getJoystickSettings(joystick);

//*********FINISHED*********--------Drive Train--------------------------------------------------------------------

    if(joystick.joy1_TopHat == 6)
    {
      if(dtactive == 0)
      {
        if(dtx == dtmin)
        {
          dtx = dtmax;
        }
        else
        {
          dtx = dtmin;
        }
        dtactive = 1;
      }
    }
    else
    {
      if(dtx == 0)
      {
        dtx = dtmin;
      }
      dtactive = 0;
    }

    float sleft = joystick.joy1_y2 * dtx;
    float sright = joystick.joy1_y1 * dtx;

    if (abs (joystick.joy1_y2) > 10)
    {
      motor[motorLeftDrive] = (sleft);
    }
    else
    {
      motor[motorLeftDrive]  = 0;
    }

    if (abs (joystick.joy1_y1) > 10)
    {
      motor[motorRightDrive] = (sright);
    }
    else
    {
      motor[motorRightDrive]  = 0;
    }
//  This controls the drivetrain. When the analog sticks are pushed forward or backwards, the robot will move forward or backwards.

//*********FINISHED*********--------Elevator-----button pressed sets direction, same button off, opposite switch
      if(joy1Btn(05) == 1 | joy1Btn(07) == 1)
      {
        if(eactive == 0)
        {
          if(joy1Btn(05) == 1)
          {
           if(elevator == elevatorspeed)
           {
             elevator = 0;
           }
           else
           {
            elevator = elevatorspeed;
           }
          }
          else
          {
           if(elevator == elevatorspeed * -1)
           {
             elevator = 0;
           }
           else
           {
            elevator = elevatorspeed * -1;
           }
          }
          eactive = 1;
        }
      }
      if(joy1Btn(05) == 0 & joy1Btn(07) == 0)
      {
        eactive = 0;
      }
      motor[motorCentralLift] = (elevator);

//  This sets the central lift on and off. It allows us to control the direction too.
//  By pressing the button once (Btn(05)), the motor will turn on. Pressing it a second time will turn the lift off.
//  The other button (Btn(07)) will flip the direction of the motor so that it will got backwards. Pressing that button again will turn it off.

//FINISHED--------Bowling Ball Catch------------------------------------------------------------------------

     if(joy1Btn(02) == 1)
     {
       if(sactive == 0)
       {
        if(ServoValue[servobb1] == servod)
        {
         servo[servobb1] = servou;
        }
        else
        {
         servo[servobb1] = servod;
        }
        sactive = 1;
       }
      }
      else
      {
        sactive = 0;
      }

//

// *********FINISHED*********--------Main Arm--------------------------------------------------------------------

    if(joystick.joy1_TopHat == 4)
    {
      if(aactive == 0)
      {
        if(as == amin)
        {
          as = amax;
        }
        else
        {
          as = amin;
        }
        aactive = 1;
      }
    }
    else
    {
      if(as == 0)
      {
        as = amin;
      }
      aactive = 0;
    }

    if(joy1Btn(04) == 1)
    {
      motor[motorRightLift] = as;
    }
    else
    {
      motor[motorRightLift] = 0;
    }
    if(joy1Btn(01) == 1)
    {
      motor[motorRightLift] = -as;
    }
    else
    {
      motor[motorRightLift] = 0;
    }

// *********FINISHED*********--------Flipper--------------------------------------------------------------------

    if(joy1Btn(06) == 1)
    {
      motor[motorLeftLift] = flips;
    }
    else
    {
      motor[motorLeftLift] = 0;
    }
    if(joy1Btn(08) == 1)
    {
      motor[motorLeftLift] = -flips;
    }
    else
    {
      motor[motorLeftLift] = 0;
    }

// *********FINISHED*********--------Servo Lock--------------------------------------------------------------------
    //  This raises and lowers the ball hopper. There are 2 modes so that it can be raised at different speeds.

     if(joy1Btn(03) == 1)
     {
       if(qactive == 0)
       {
        if(ServoValue[servobb2] == servoq)
        {
         servo[servobb2] = servoqd;
        }
        else
        {
         servo[servobb2] = servoq;
        }
        qactive = 1;
       }
      }
      else
      {
        qactive = 0;
      }
 }
}

if(joy1Btn(04) == 1)
    {
      motor[motorRightLift] = as;
    }
    else
    {
      motor[motorRightLift] = 0;
    }
    if(joy1Btn(01) == 1)
    {
      motor[motorRightLift] = -as;
    }
    else
    {
      motor[motorRightLift] = 0;
    }

    if ( joy1Btn(04) == 1 )
    {
      motor[motorRightLift] = as;
    }
    else if ( joy1Btn(01) == 1 )
    {
      motor[motorRightLift] = -as;
    }
    else  // only when *neither* button is pressed:
    {
      motor[motorRightLift] = 0;
    }