#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  motorA,          ,              tmotorNormal, PIDControl)
#pragma config(Motor,  motorB,          ,              tmotorNormal, PIDControl)
#pragma config(Motor,  motorC,          ,              tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)

#include "JoystickDriver.c"
task main()
{
  while(true)  //continously loop to check for "mode" switching
  {
    getJoystickSettings(joystick);  //update joystick values from FMS
    while(joystick.UserMode = false) //While in "autonomous" mode (UserMode = false)
    {
        while(joystick.StopPgm)  //While the FMS says we're "disabled", keep looping
        {
          getJoystickSettings(joystick);  //Update joystick/game values to see if enabled
          wait1Msec(50);  //wait 50 milliseconds
       }
     //Autonomous Code Here
   }
   while(joystick.UserMode = true) //While in "User Control" mode (UserMode = false)
   {
      getJoystickSettings(joystick);  //Keep Updating Joysticks
      //User Code Here
        {
           while(true)
            {
              {
                motor[motorA] = 100;  //These two lines turn on the intake, it keeps running nonstop
                motor[motorB] = 100;
              }
              getJoystickSettings(joystick);
              motor[motorD] = joystick.joy1_y1; //Joystick y1 controls drive motor D
              getJoystickSettings(joystick);
              motor[motorE] = joystick.joy1_y2; //Joystick y2 controls drive motor E
              getJoystickSettings(joystick);

              if(joy1Btn(10))  //If Joystick Button #10 is pressed raise extender arms..
             {
               servoTarget[servo2] = 0;
             	 servoTarget[servo1] = 255;
             }
             else //If Joystick Button #10 is not pressed, do nothing
             {
             }
             if(joy1Btn(09))  //If Joystick Button #09 is pressed, lower extender arms
             {
             	servoTarget[servo2] = 255;
             	servoTarget[servo1] = 0;
             }
              else //If Joystick Button #09 is not pressed, do nothing
             {
             }
             if(joy1Btn(6)) //This is controlling the tipper...
             {
                servoTarget[servo3] = 0; // up is the default state
             }
              else
             {
               servoTarget[servo3] = 255; //button pressed, arm spins down.
             }
             if(joy1Btn(1) != 0) // This is controlling the Veltcro
             {
                motor[motorC] = 100;  //button 1 pressed, belt runs up..
             }
              else
             {
               motor[motorC] = 0;
             }
           }     //no reverse?
          }
   //Shucks.
   }
  }
}