#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  HTMotor)
#pragma config(Motor,  mtr_S1_C1_1,     tread_left,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     belt_left,     tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     tread_right,   tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     belt_right,    tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C4_1,     crane_arm,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     crane_lift,    tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    tray_top,             tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    extender_right,       tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    extender_left,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    magnetic,             tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    crane,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
CONTROLLER ASSIGNMENTS:
  Controler I (Driver)
    Left Joystick
      Y Axis: tread_left/crane_arm
    Right Joystick
      Y Axis: tread_right/crane_lift
    Button 5: Hold down to enable crane_arm (during crane mode)
    Button 6: Nitro boost
    Button 9: Change Joystick Mode
  Controller II (Assistant)
    Right Joystick:
      Y Axis: Conveyor Belts (both synced)
    Button 2: Top Tray Closed
    Button 4: Top Tray Open
    TopHat 0: Front Extenders Down
    TopHat 4: Front Extenders Up
*/

// CONFIG [start]
int throttle = 20;
int cranemode = 0;
int cranearmpwr = 20;
int craneliftpwr = 25;
  // SERVO CONFIG [start]
  // 0 = FIRST POSITION (IN, DISABLED, OR CLOSED)
  // 1 = SECOND POSITION (OUT, ENABLED, OR OPEN)
  int srvcrane0 = 0;
  int srvcrane1 = 255;
  int srvextendersL0 = 0;
  int srvextendersL1 = 255;
  int srvextendersR0 = 255;
  int srvextendersR1 = 0;
  int srvtoptray0 = 0;
  int srvtoptray1 = 255;
  int srvmagnetic0 = 0;
  int srvmagnetic1 = 255;
  // SERVO CONFIG [end]
// CONFIG [end]

int nitro=throttle;
int srvextenders=4;
int J1Btn9=0;

#include "JoystickDriver.c"       // call joystick driver

task main()
{
  while(true)
  {
    getJoystickSettings(joystick);      // get joystick settings
// CONTROLLER I (DRIVER) [start]
    if(joy1Btn(9) == 0) {J1Btn9 = 0;}     // if Button 9 is not pressed reset count to 0
    if(joy1Btn(9) == 1) {J1Btn9++;}       // if Button 9 is pressed add 1 to count
    if(J1Btn9 == 1)                      // if and only if J1Btn9 count is 1 then:
      {
        if(cranemode == 0) {cranemode=2;}       // if cranemode is off switch it on using Triangle Method
        if(cranemode == 1) {cranemode=0;}       // if cranemode is on switch it off
        if(cranemode == 2) {cranemode=1;}       // if Triangle Method has a temporary value, switch cranemode on
      }
    if(cranemode == 0)          // if cranemode is off then turn on Driving Mode
    {
      if(joy1Btn(6) == 1)       // if Button 6 is pressed NITRO on (play tone)
      {
        if(nitro == throttle)
          {
            PlayTone(174.61, 10);
            PlayTone(261.63, 10);
            PlayTone(349.23, 10);
           }
       nitro = 100;
      }
       if(joy1Btn(6) == 0)      // if Button 6 is not pressed NITRO off (play tone)
      {
           if(nitro == 100)
          {
            PlayTone(349.23, 10);
            PlayTone(261.63, 10);
            PlayTone(174.61, 10);
           }
       nitro = throttle;
      }
    motor[tread_right] = nitro*(joystick.joy1_y2/100);  // move motor according to corresponding joystick
    motor[tread_left] = nitro*(joystick.joy1_y1/100);   // move motor according to corresponding joystick
    }
    if(cranemode == 1)                                  // if cranemode is on then turn off Driving Mode
    {
      if(joy1Btn(5) == 1) {motor[crane_arm] = cranearmpwr*(joystick.joy1_y1/100);}
      motor[crane_lift] = craneliftpwr*(joystick.joy1_y2/100);}
    }
// CONTROLLER I (DRIVER) [end]
// CONTROLLER II (ASSISTANT) [start]

     motor[belt_left] = joystick.joy2_y2;
     motor[belt_right] = joystick.joy2_y2;

     if(joy2Btn(2) == 1) {servo[tray_top] = 255;}        // If Button 1 is pressed, open top tray servo
     if(joy2Btn(4) == 1) {servo[tray_top] = 0;}          // If Button 2 is pressed, close top tray servo
     if(joystick.joy2_TopHat == 0) {srvextenders = 0;}   // If TopHat 0 is pressed, set extender servos to go in
     if(joystick.joy2_TopHat == 4) {srvextenders = 4;}   // If TopHat 4 is pressed, set extender servos to go out
     if(srvextenders == 0)                        // If extender servos are set to move out:
     {
       servo[extender_left] = srvextendersL1;     // Move left extender down and out
       servo[extender_right] = srvextendersR1;    // Move right extender down and out
     }
     if(srvextenders == 4)                        // If extender servos are set to move in:
     {
       servo[extender_left] = srvextendersL0;     // Move left extender up and in
       servo[extender_right] = srvextendersR0;    // Move right extender up and in
     }
// CONTROLLER II (ASSISTANT) [end]
}