#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include the Joystick/Game variables driver.

task main()
{
   getJoystickSettings(joystick);  //Updates the game and joystick variables

        while(joystick.StopPgm)  //Continues this loop while the robot is disabled (StopPgm = true)
        {
           getJoystickSettings(joystick);  //Checks to see if the robot has been enabled (StopPgm = false)
           wait1Msec(50);  //Waits 50 milliseconds for a new BT packet to come.
        }

       // Autonomous Code Below - You can put all of your autonomous program below

   motor[motorD] = 50;
   motor[motorE] = 50;
   wait1Msec(3000);

   motor[motorD] = 0;
   motor[motorE] = 0;
}

task main()
{
  // assumes RobotC version 1.44
  while (true)
  {
    getJoystickSettings(joystick);

    // has the FMS stopped the program
    if (joystick.StopPgm)
    {
      nxtDisplayTextLine(2, "Disabled");
      wait1Msec(50);
      stopMotors();   // this routine sets all active motors to 0 - exercise left to student.
      continue;   // go back into top of while loop
    }

    if (!joystick.UserMode)    // autonomous mode
    {
      nxtDisplayTextLine(2, "Autonomous");
          // put autonomous code here
    }

    else                      // teleop mode
    {
      nxtDisplayTextLine(2, "TeleOp");
          // put teleop code here
    }
  }
}