#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S2, HTServo,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftdrive,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     rightdrive,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     armupdown,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     armleftright,  tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorJ,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorK,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    claw,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    unkwown,              tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    servo7,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_2,    servo8,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////ROBOT///////////////////////////////////////////
/*///////////////////////////////////////////////////////////////////////////////////////
/Controls                        Description                              othernames    /
/////////////////////////////////////////////////////////////////////////////////////////
/topHat------------used to control the claw being open or closed------------topHat      /
/Button1-----------used to make the arm turn Left---------------------------armleft     /
/Button2-----------used to make the arm go up-------------------------------armup       /
/Button3-----------used to make the arm go down-----------------------------armdown     /
/Button4-----------used to make the arm turn right--------------------------armright    /
/Joystick1---------uesd to make the robot move------------------------------???????     /
/Joystick2---------used to make the robot move------------------------------???????     /
/triggerL1---------???????????????????????????------------------------------???????     /
/triggerL2---------???????????????????????????------------------------------???????     /
/triggerR1---------???????????????????????????------------------------------???????     /
/triggerR2---------???????????????????????????------------------------------???????     /
*////////////////////////////////////////////////////////////////////////////////////////
const int armup = 2;
const int armdown = 3;
const int armleft = 1;
const int armright = 4;
const int threshold = 5;
#include "JoystickDriver.c"

void triggers()
{
  //trigger controls for claw (the claw is a VEX Robotics claw)
}
void handleArm()
{
  //up/down
                                   if (joy1_Btn(armup))
                                   {
                                      motor[armupdown] = 25; //motor that lifts the arm goes up
                                   }
  else if (joy1_Btn(armdown))
  {
    motor[armupdown] = -25; //motor that lifts the arm goes down
  }
  else
  {
    motor[armupdown] = 0; //motor that lifts the arm is stationary
  }
  //left/right
  if (joy1_Btn(armleft))
  {
    motor[armleftright] = 10; //motor that turns the arm moves to the left
  }
  else if (joy1_Btn(armright))
  {
    motor[armleftright] = -10; //motor that turns the arm moves to the right
  }
  else
  {
    motor[armleftright] = 0; //motor that turns the arm is stationary
  }
}
void tankDrive()
{
  //put tank drive code here
  //forward/backward
  //turn left/right
    if(abs(joystick.joy1_y2) > threshold)   // If the right analog stick's Y-axis readings are either above or below the threshold:
    {
      motor[rightdrive] = joystick.joy1_y2;         // Motor rightdrive is assigned a power level equal to the right analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the threshold:
    {
      motor[rightdrive] = 0;                        // Motor rightdrive is stopped with a power level of 0.
    }


    if(abs(joystick.joy1_y1) > threshold)   // If the left analog stick's Y-axis readings are either above or below the threshold:
    {
      motor[leftdrive] = joystick.joy1_y1;         // Motor leftdrive is assigned a power level equal to the left analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the threshold:
    {
      motor[leftdrive] = 0;                        // Motor leftdrive is stopped with a power level of 0.
    }
}

task main()
{
while (true)
{
 getJoystickSettings(joystick);
 handleArm();
 tankDrive();
 triggers();
}
}