#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"

task main()
{
  int threshold = 20;             // Threshold is the area that the motor won't activate until the value is greater
                                  // than the threshold. This is so we don't have to edit the threshold

  while(true)                            // keeps loop going forever
  {
    getJoystickSettings(joystick);
    if(abs(joystick.joy1_y2) > threshold)   // If the right analog stick's Y-axis readings are either above or below the threshold:
    {
      motor[motorD] = joystick.joy1_y2;         // Motor D 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[motorD] = 0;                        // Motor D 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[motorE] = joystick.joy1_y1;         // Motor E 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[motorE] = 0;                        // Motor E is stopped with a power level of 0.
    }


    // BUTTONS TO CONTOL SERVO ARM
    // button 5 makes servo goes up, 6 goes down.

    if(joy1Btn(5))          // If Button 5 is pressed:
    {
      servo[servo1] = 245;      // Raise Servo 1 to position 200.
    }

    if(joy1Btn(6))          // If Button 6 is pressed:
    {
      servo[servo1] = 10;       // Lower Servo 1 to position 20.
    }
  }
}

 //
 // The BOUND macro limits the value (n) within the bounds between the given
 // low (l) and high (h).
 //
#define BOUND(n,l,h)            (((n) < (l))? (l): ((n) > (h))? (h): (n))

//
// The NORMALIZE macro transforms a value (n) in the range between (sl) and
// (sh) to the range between (tl) and (th).
//
#define NORMALIZE(n,sl,sh,tl,th) (int)(((long)(n) - (sl))*((th) - (tl))/((sh) - (sl)) + (tl))

//
// Joystick macros.
//
#ifndef DEADBAND_INPUT_THRESHOLD
    #define DEADBAND_INPUT_THRESHOLD 20
#endif

//
// These macros ignore input value (n) that is within the DEADBAND_THRESHOLD.
// This is necessary because analog joysticks do not always centered at zero.
// So if the joystick is at the rest position, we will consider it zero even
// though the value is non-zero but within DEADBAND_THRESHOLD.
//
#define DEADBAND(n,t)           ((abs(n) > (t))? (n): 0)
#define DEADBAND_INPUT(n)       DEADBAND(n, DEADBAND_INPUT_THRESHOLD)

    //
    // This basically scales the joystick range (-128 to 127) to whatever
    // range you want to limit (-50 to 50 in your case).
    //
    motor[motorE] = NORMALIZE(DEADBAND_INPUT(joystick.joy1_y1), -128, 127, -50, 50);
    motor[motorD] = NORMALIZE(DEADBAND_INPUT(joystick.joy1_y2), -128, 127, -50, 50);

#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed)