#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTTMUX,         sensorAnalogInactive)
#pragma config(Sensor, S3,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S4,     HTCOLOR,        sensorCOLORFULL)
#pragma config(Motor,  mtr_S1_C1_1,     rightDrive,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftDrive,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     liftLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     liftRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     arm,           tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    rightGate,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftGate,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    pusher,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////                                                                                                    //////////
//////////                                       OUR AMAZING TELEOP CODE                                      //////////
//////////                                   BY: Iman Nandi and Frank Zhou                                    //////////
//////////                                                                                                    //////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//#include "LEGOLS-driver.h"
#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "drivers/common.h"
#include "drivers/hitechnic-touchmux.h"
#include "drivers/hitechnic-sensormux.h"
#include "hitechnic-colour-v2.h"
#include "drivers/lego-light.h"
#include "drivers/hitechnic-force.h"

const tMUXSensor LEGOLS = msensor_S3_1;
const tMUXSensor HTFORCE = msensor_S3_4;

void initializeRobot() //initialize the robot
{
   alive(); //calibrate();
   servo(leftGate) = 0; // set the servo controlled ring gates to 0
   servo(rightGate) = 255;
   nMotorEncoder[rightDrive] = 0;//reset the value of encoder B to zero
   nMotorEncoder[leftDrive] = 0;
   return;
}
task main()
{
int _force = 0;
while (true)//the while loop of teleop
   {

      getJoystickSettings(joystick);//get the layout of the joystick for programming the correct joystick model
      _force = HTFreadSensor(HTFORCE);
                 writeDebugStreamLine("force: %d", HTFreadSensor(HTFORCE));
      //writeDebugStreamLine("forcev: %d", SensorValue(HTFORCE));
              if (50 >= _force > 45)
      {
         heavyRing = true;
         int force = _force;
         nxtDisplayCenteredTextLine(3,"%d, %d","true",force);
      }
      else if (_force > 50 || 45 >= _force)
      {
         heavyRing = false;
      }
      if(_force > 50)
      {
         twoHeavyRings = true;
      }
      else if(50 >= _force)
      {
         twoHeavyRings = false;
      }
if(heavyRing == true)
      {
         SensorType[HTCOLOR] = sensorCOLORRED;
         //PlayImmediateTone(500,3);
      }
      else if(twoHeavyRings == true)
      {
         SensorType[HTCOLOR] = sensorCOLORBLUE;
         //PlayImmediateTone(800,3);
      }
      else
      {
         SensorType[HTCOLOR] = sensorCOLORNONE;
      }
}