#pragma config(Hubs,  S1, HTServo,  HTServo,  HTMotor,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C3_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorE,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_1,     motorF,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     motorG,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C1_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_1,    servo7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo10,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo11,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo12,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "drivers/HTSMUX-driver.h"
#include "drivers/HTMAG-driver.h"

const tMUXSensor HTMAG = msensor_S2_4;

void initializeRobot()
{
  // calibrate the Magnetic sensor
  HTMAGstartCal(HTMAG);

  //from the front of the robot
  servo[servo1] = 138; //right bowling ball servo up pos
  servo[servo12] = 96; //left bowling ball servo up pos

  servo[servo7] = 0; //from the front, the left hand servo is a 0 when the arm is extended. It is at 229 when it is at its lowest.
  servo[servo8] = 0; //from the front, the right hand servo is a 26 when the arm is extended. It is at 255 when it is at its lowest.

  servo[servo9] = 225; //small servo joint

  servo[servo11] = 255; //arm lock mechanism down

  servo[servo2] = 20; //magnet indicator flag down

  return;
}

task main()
{
  nNoMessageCounterLimit = 200;
  initializeRobot();

  waitForStart();

  int Joint1ServoRightPos = 0;
  int Joint1ServoLeftPos = 0;
  int Joint2Pos = 225; //open pos

  while (true)
  {
    getJoystickSettings(joystick);
     if(abs(HTMAGreadVal(HTMAG))>5){
      servo[servo2] = 135;
    }
    else{
      servo[servo2] = 20;
    }

    //The following code programs which motors should be controlled by the joysticks
    //motor is the left hand motor and motor is the right hand motor
    motor[motorE] = -joystick.joy1_y1;
    motor[motorD] = joystick.joy1_y2;

    //servo lock button
    if(joy2Btn(4) == 1)
    {
      servo[servo11] = 23;
    }

    if(joy2Btn(2) == 1)
    {
      servo[servo11] = 255;
    }

    //servo 1 is the right hand bowling ball servo (from the front of the robot)
    //servo 12 is the left hand bowling ball servo (from the front of the robot)
    if(joy1Btn(1) == 1)
    {
      servo[servo1] = 138;
      servo[servo12] = 96;
      //up
    }

    if(joy1Btn(2) == 1)
    {
      servo[servo1] = 5;
      servo[servo12] = 240;
      //down
    }

    //The following code programs the smaller, 2nd joint of the arm
    if(joy2Btn(5) == 1)
    {
     Joint2Pos = Joint2Pos+1;
     wait1Msec(5);
     //close
    }

    if(joy2Btn(6) == 1)
    {
      Joint2Pos = Joint2Pos-1;
      wait1Msec(5);
      //open
    }

    servo[servo9] = Joint2Pos;


    //The following code programs the larger, 1st joint of the arm
    if(joy2Btn(8) == 1)
    {
     Joint1ServoLeftPos = Joint1ServoLeftPos+1;
     Joint1ServoRightPos = Joint1ServoRightPos+1;
     wait1Msec(5);
     //open
    }

    if(joy2Btn(7) == 1)
    {
      Joint1ServoLeftPos = Joint1ServoLeftPos-1;
      Joint1ServoRightPos = Joint1ServoRightPos-1;
      wait1Msec(5);
      //close
    }

    servo[servo7] = Joint1ServoLeftPos;
    servo[servo8] = Joint1ServoRightPos;

    //The following code programs the DC motor at the base
    motor[motorF] = -(1/2.56)*joystick.joy2_y2;

  }
}

// calibrate the Magnetic sensor
  HTMAGstartCal(HTMAG);

  nNoMessageCounterLimit = 200;
  initializeRobot();[

while (true)
  {
    getJoystickSettings(joystick);
     if(abs(HTMAGreadVal(HTMAG))>5){
      servo[servo2] = 135;
    }
    else{
      servo[servo2] = 20;