#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Motor,  motorA,          motor1,        tmotorNormal, PIDControl)
#pragma config(Motor,  motorB,          motor2,        tmotorNormal, PIDControl)
#pragma config(Motor,  motorC,          motor3,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     motorF,        tmotorNormal, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Servo,  servo1,          ServoC,               tServoNormal)
#pragma config(Servo,  servo2,          ServoD,               tServoNormal)
#pragma config(Servo,  servo5,          ServoA,               tServoNormal)
#pragma config(Servo,  servo6,          ServoB,               tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

task main()
{
  while (true)
  {
    getJoystickSettings(joystick);
    motor[motorF] = joystick.joy2_y1;
    motor[motorG] = joystick.joy2_y2;

    nMotorEncoder[motorD] = 0;
    nMotorEncoder[motorE] = 0;

    if(joy1Btn(1)) //If Joy1-Button1 is pressed
    {
      while(nMotorEncoder[motorD] < 250 || nMotorEncoder[motorE] < 250)
    {
      motor[motorD] = 25;
      motor[motorE] = 25;
    }

    motor[motorD] = 0;
    motor[motorE] = 0;
    wait1Msec(1000);

    }
    else //If Joy1-Button1 is NOT pressed
    {
      motor[motorD] = 0; // Turn Motor A Off
      motor[motorE] = 0; // Turn Motor A On at full power
    }
       if(joy1Btn(2)) //If Joy1-Button1 is pressed
        {
       nMotorEncoderTarget[motorD] = 250;
       nMotorEncoderTarget[motorE] = 250;

      motor[motorD] = -50;
      motor[motorE] = -50;

      while(nMotorRunState[motorD] != runStateIdle || nMotorRunState[motorE] != runStateIdle)
      {

      }
      }




    else //If Joy1-Button1 is NOT pressed
    {
      motor[motorD] = 0; // Turn Motor A Off
      motor[motorE] = 0; // Turn Motor A On at full power
    }


    if(joy1Btn(6)) //If Joy1-Button1 is pressed
    {
      motor[motor1] = -100; // Turn Motor A On at full power
      motor[motor2] = -100; // Turn Motor A Off
      motor[motor3] = -100;

    }
    else
    {
      motor[motor1] = 0;
      motor[motor2] = 0;
      motor[motor3] = 0;
    }
      if(joy1Btn(5)) //If Joy1-Button2 is pressed
    {
      servoTarget[ServoA] = 240; // Move Servo1 to position '240'
      servoTarget[ServoB] = 18;
    }
    else //If Joy1-Button2 is NOT pressed
    {
      servoTarget[ServoA] = 128; // Move Servo1 to position '50'
      servoTarget[ServoB] = 128;
    }


    if(joy1Btn(7)) //If Joy1-Button1 is pressed
    {
      motor[motor1] = 100; // Turn Motor A On at full power
      motor[motor2] = 100; // Turn Motor A On at full power
      motor[motor3] = 100;

    }
    else //If Joy1-Button1 is NOT pressed
    {
      motor[motor1] = 0; // Turn Motor A Off
      motor[motor2] = 0; // Turn Motor A On at full power
      motor[motor3] = 0;
    }
    if(joy1Btn(5)) //If Joy1-Button2 is pressed
    {
      servoTarget[ServoA] = 120; // Move Servo1 to position '240'
      servoTarget[ServoB] = 150;
    }
    else //If Joy1-Button2 is NOT pressed
    {
      servoTarget[ServoA] = 0; // Move Servo1 to position '50'
      servoTarget[ServoB] = 240;
    }
     if(joy1Btn(3)) //If Joy1-Button2 is pressed
    {
      servoTarget[ServoC] = 100; // Move Servo1 to position '240'
      servoTarget[ServoD] = 175;
    }
    else //If Joy1-Button2 is NOT pressed
    {
      servoTarget[ServoC] = 275; // Move Servo1 to position '50'
      servoTarget[ServoD] = 55;
    }
  }
}

      if(joy1Btn(5)) //If Joy1-Button2 is pressed
    {
      servoTarget[ServoA] = 240; // Move Servo1 to position '240'
      servoTarget[ServoB] = 18;
    }
    else //If Joy1-Button2 is NOT pressed
    {
      servoTarget[ServoA] = 128; // Move Servo1 to position '50'
      servoTarget[ServoB] = 128;
    }

 if(joy1Btn(5)) //If Joy1-Button2 is pressed
    {
      servoTarget[ServoA] = 120; // Move Servo1 to position '240'
      servoTarget[ServoB] = 150;
    }
    else //If Joy1-Button2 is NOT pressed
    {
      servoTarget[ServoA] = 0; // Move Servo1 to position '50'
      servoTarget[ServoB] = 240;
    }