#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, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     MotorE,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C3_1,     MotorF,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     MotorG,        tmotorNormal, openLoop)
#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               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {



{
  while (true)
  {
    getJoystickSettings(joystick);
    motor[MotorG] = joystick.joy2_y1;
    motor[MotorF] = joystick.joy2_y2;

    nMotorEncoder[MotorD] = 0;
    nMotorEncoder[MotorE] = 0;

    if(joy1Btn(1))
    {
      while(nMotorEncoder[MotorD] < 100 || nMotorEncoder[MotorE] < 100)
    {
      motor[MotorD] = 25;
      motor[MotorE] = 25;
    }

    motor[MotorD] = 0;
    motor[MotorE] = 0;
    wait1Msec(1000);

    }
    else
    {
      motor[MotorD] = 0;
      motor[MotorE] = 0;
    }
       if(joy1Btn(2))
        {
       nMotorEncoderTarget[MotorD] = 100;
       nMotorEncoderTarget[MotorE] = 100;

      motor[MotorD] = -75;
      motor[MotorE] = -75;

      while(nMotorRunState[MotorD] != runStateIdle || nMotorRunState[MotorE] != runStateIdle)
      {

      }
      }

    else
    {
      motor[MotorD] = 0;
      motor[MotorE] = 0;
    }

    if(joy2Btn(2))
    {
      motor[Motor1] = -100;
      motor[Motor2] = -100;
      motor[Motor3] = -100;

    }
    else
    {
      motor[Motor1] = 0;
      motor[Motor2] = 0;
      motor[Motor3] = 0;
    }



    if(joy2Btn(1))
    {
      motor[Motor1] = 100;
      motor[Motor2] = 100;
      motor[Motor3] = 100;

    }
    else
    {
      motor[Motor1] = 0;
      motor[Motor2] = 0;
      motor[Motor3] = 0;
    }
    if(joy1Btn(5))
    {
      servoTarget[ServoA] = 220;
      servoTarget[ServoB] = 80;
    }
    else
    {
      servoTarget[ServoA] = 20;
      servoTarget[ServoB] = 220;
    }
     if(joy2Btn(3))
    {
      servoTarget[ServoC] = 100;
      servoTarget[ServoD] = 175;
    }
    else
    {
      servoTarget[ServoC] = 275;
      servoTarget[ServoD] = 0;
    }
  }
}

  }
}