#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1,     drivemotor1,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     drivemotor2,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_1,    servo7,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    servo8,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo10,              tServoStandard)
#pragma config(Servo,  srvo_S2_C2_5,    servo11,              tServoStandard)
#pragma config(Servo,  srvo_S2_C2_6,    servo12,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c" //Joystick is included in this program

task main()
{
  int threshold = 15; //to avoid unnecessary movement

  servo[servo1]=50;
  wait1Msec(1000);

  while(true) //infinite loop
  {
    getJoystickSettings(joystick); //retrieves data from the joystick

    if(abs(joystick.joy1_y1) > threshold)
    {
      motor[drivemotor1] = joystick.joy1_y1; //y1 controller moves drive motor
    }
    else
    {
      motor[drivemotor1] = 0;
    }


    if(abs(joystick.joy1_y2) > threshold)
    {
      motor[drivemotor2] = joystick.joy1_y2; //y2 controller moves motorE
    }
    else
    {
      motor[drivemotor2] = 0;
    }
    if(joystick.joy1_Buttons == 128)
    {
      servo[servo1] = ServoValue[servo1]+3;
    }
    if(joystick.joy1_Buttons == 32)
    {
      servo[servo1] = ServoValue[servo1]-3;
    }
    if(joystick.joy1_Buttons == 2)
    {
    servo[servo2] = ServoValue[servo2]-5;
    }
    if(joystick.joy1_Buttons == 4)
    {
      servo[servo2] = ServoValue[servo2]+5;
    }
   }
}