#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     ,                    sensorLightActive)
#pragma config(Motor,  motorA,          motorA,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     Right,         tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     Left,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     Lift,          tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     MotorG,          tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    leftflap3,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    rightflap4,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_1,    L7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    R8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[lift2] = 0;
  servoTarget[leftflap3] = 0;
  servoTarget[rightflap4] = 0;
  servoTarget[L7] = 0;
  servoTarget[R8] = 0;
  servoTarget[MagR5] = 0;

  return;
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 670) // Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else if(SensorValue[HTMAG] < 660)// Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else
    {
      servoTarget[Mag1] = 127;
    }
}

task main()
{
  initializeRobot();

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

  while (true)
  {

    getJoystickSettings(joystick);
    processMagnet();
    motor[Left] = joystick.joy1_y1;
    motor[Right] = joystick.joy1_y2;
    motor[Lift] = joystick.joy2_y1;// Ball Lift
    motor[motorG] = joystick.joy2_y2;

    //******************Lift Lock (End Game)******************
   if (joy2Btn(3))
    {
    servoTarget[lift2] = 255;//Locked
    }
   else
    {
     servoTarget[lift2] = 0; //Opened
   }
    //******************Left Flap******************
   if (joy1Btn(5))
    {
    servoTarget[leftflap3] = 60;//Out
    }
   else
    {
     servoTarget[leftflap3] = 0; //closed
   }
   //******************Right Flap******************
   if (joy1Btn(6))
    {
    servoTarget[rightflap4] = 60;//Out
    }
   else
    {
     servoTarget[rightflap4] = 0; //closed
   }
    //******************Lift Left Servos******************
   if (joy2Btn(5))
    {
    servoTarget[L7] = 255;//Open
    }
   else
    {
     servoTarget[L7] = 10; //Closed
   }
   //******************Lift Right Servos******************
   if (joy2Btn(6))
    {
    servoTarget[R8] = 0;
    }
   else
    {
     servoTarget[R8] = 245;
   }


   //******************Magnetic Release******************
   if (joy2Btn(2))
    {
    servoTarget[MagR5] = 60;//Out
    }
   else
    {
     servoTarget[MagR5] = 0; //closed
   }
 }
}

bool btn5Pressed = false;
bool leftFlapIsUp = false;
while(true){
//code
  if(!btn5Pressed && joy1Btn(5))
  {
    leftFlapIsUp = !leftFlapIsUp;
    btn5Pressed = true;
  }
  else if(!joy1Btn(5))
  {
    btn5Pressed = false;
  }
  if(leftFlapIsUp){
    servoTarget[leftflap3] = 60;
  }
  else
  {
    servoTarget[leftflap3] = 0;
  }
//code
}