#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     compass,        sensorNone)
#pragma config(Sensor, S3,     light,          sensorLightActive)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motorA,        tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     motorB,        tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S2_C1_1,     motorC,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorD,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C2_1,    armServo1,            tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    armServo2,            tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    clawServo,            tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define DEADBAND_THRESHOLD 10       //*!!Define as 10*//
#define BOUND(x) (((x) > 100)? 100: ((x) < -100)? -100: (x))     //*!!Makes sure that the joystick is between -100 and 100 !!*//
#define DEADBAND(x) ((abs(x) >= DEADBAND_THRESHOLD)? x: 0)     //*!!If the joystick is between -10 and 10, it will be treated as 0 !!*//

#define TIME_INTERVAL 100

void arm_servos( int joy2_y1 )
{
   int k = 127;
   if ( DEADBAND(joy2_y1) > 0 )
  {
     servo[armServo1] = k;
     servo[armServo2] = k;
  }
  else
  {
     servo[armServo1] = 0;
     servo[armServo2] = 0;
  }
}

void claw_servo ( int btn2, int btn4 )
{
   int k = 127;
  if ( btn2 == 1 )
   {
      servo[ clawServo ] = k;
   } else
   if ( btn4 == 1 )
   {
      servo[ clawServo ] = 0;
   }
}

#include "JoystickDriver.c"
task main()
{
    while(true)
    {
        getJoystickSettings(joystick); // Update Buttons and Joysticks

        arm_servos( joystick.joy2_y1 );
        claw_servo( joy2Btn(2), joy2Btn(4) );
        wait1Msec(TIME_INTERVAL);
    }
}