#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,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    ,                     tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"

task main()
{


  servo[servo1] = 255;
  servo[servo2] = 14;
  servo[servo3] = 0;
    for(;;)
  {
    getJoystickSettings(joystick);

// Move motors with joystick and prevent drifting
    if(joystick.joy1_y1 < 5 && joystick.joy1_y1 > -5)
      {
        motor[motorD] = 0;
      }
    else
      {
        motor[motorD] = joystick.joy1_y1;
      }
    if(joystick.joy1_y2 < 5 && joystick.joy1_y2 > -5)
      {
        motor[motorE] = 0;
      }
    else
      {
        motor[motorE] = joystick.joy1_y2;
      }

// Joystick button configuration
    if(joy1Btn(7) != 0) //If Joy1-Button7 is pressed
      {
        servo[servo1] = 87; // Move Arm to "down" position
        servo[servo2] = 173;
      }
    if(joy1Btn(5) != 0) //If Joy1-Button5 is pressed
      {
        servo[servo1] = 255;
        servo[servo2] = 14; // Move Arm to "up" position
      }
    if(joy1Btn(6) != 0) //If Joy1-Button6 is pressed move arm up slowly
      {
        servo[servo1] = ServoValue [servo1] + 1; // Adds an Integer to servo1's current position
        servo[servo2] = ServoValue [servo2] - 1; // Adds an Integer to servo1's current position
      }
    if(joy1Btn(8) != 0) //If Joy1-Button8 is pressed move arm down slowly
      {
        servo[servo1] = ServoValue [servo1] - 1; // Subtracts an Integer to servo1's current position
        servo[servo2] = ServoValue [servo2] + 1; // Subtracts an Integer to servo1's current position
      }
    if(joy1Btn(3) !=0) //move back hook up slowly
      {
        servo[servo3] = ServoValue [servo3] - 1;
      }
    if(joy1Btn(2) !=0) //move back hook down slowly
      {
        servo[servo3] = ServoValue [servo3] + 1;
      }
    if(joy1Btn(1) !=0) //move back hook down
      {
        servo[servo3] = 120;
      }
    if(joy1Btn(4) !=0) //move back hook up
      {
        servo[servo3] = 0;
      }
    if(joy2Btn(5) !=0) //rotate top dispenser left
      {
        servo[servo4] = 0;
      }
    if(joy2Btn(6) !=0) //rotate top dispenser right
      {
        servo[servo4] = 255;
      }
    else //halt servo if button isnt pressed
      {
        servo[servo4] = 127;
      }
     
     
// CONTROLLER 2 MOTOR CONTROL (TMP TESTING CODE)
     if(joystick.joy2_y1 < 5 && joystick.joy2_y1 > -5)
      {
        motor[motorD] = 0;
      }
    else
      {
        motor[motorD] = joystick.joy2_y1;
      }
    if(joystick.joy2_y2 < 5 && joystick.joy2_y2 > -5)
      {
        motor[motorE] = 0;
      }
    else
      {
        motor[motorE] = joystick.joy2_y2;
      }
  }
}