#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#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,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    Sorter,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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


void initializeRobot()
{
servo[Sorter]= 0;

  return;
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 670)
    {
        servo[Sorter] = 127;
    }
    else if(SensorValue[HTMAG] < 665)
    {
        servo[Sorter] = 255;
    }
    else
    {
        servo[Sorter] = 0;
    }
}

task main()
{
  initializeRobot();

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

  while (true)
  {

    motor[motorD] = joystick.joy1_y1;//Driving Base Right
    motor[motorE] = joystick.joy1_y2;//Driving Base Left
    motor[motorG] = joystick.joy2_y1;// Arm

   if(joy1Btn(8))//Baton Wheel 1st
   {
    motor[motorA] = 75;
   }
   else if(joy1Btn(7))//Baton Wheel 2nd
   {
     motor[motorA] = -75;
   }
   else
   {
     motor[motorA] = 0;
   }
   if(joy1Btn(1))//Bucket Level, Angle 1 Shake
    {
      servoTarget[servo4] = 78;
      servoTarget[servo5] = 160;
    }
   else if (joy2Btn(1))//Bucket Level, Angle 1
    {
      servoTarget[servo4] = 68;
      servoTarget[servo5] = 170;
    }
   else if (joy2Btn(2))//Bucket Level, Angle 2
    {
      servoTarget[servo4] = 101;
      servoTarget[servo5] = 143;
    }
   else if (joy1Btn(3))//Bucket Level, Angle 3 Shake
    {
      servoTarget[servo4] = 127;
      servoTarget[servo5] = 114;
    }
   else if (joy2Btn(3))//Bucket Level, Angle 3
    {
      servoTarget[servo4] = 117;
      servoTarget[servo5] = 124;
    }
   else if(joy2Btn(6))//Bucket Twist Right Goal
    {
      servoTarget[servo4] = 61;
      servoTarget[servo5] = 155;
      servoTarget[servo6] = 76;
    }
   else if (joy2Btn(5))//Bucket Twist Left Goal
    {
      servoTarget[servo4] = 82;
      servoTarget[servo5] = 174;
      servoTarget[servo6] = 155;
   }
   else if (joy2Btn(8))//Bucket Twist Back Goal
    {
    servoTarget[servo6] = 20;
   }
   else
    {
      servoTarget[servo4] = 105;
      servoTarget[servo5] = 137;
      servoTarget[servo6] = 129;
 }
   if (joy1Btn(4))//Flap
    {
    servoTarget[servo3] = 134;
   }
   else
   {
     servoTarget[servo3] = 14;
   }
   if (joy1Btn(5))//Trailer
    {
    servoTarget[servo2] = 143;
   }
   else if (joy1Btn(6))
    {
    servoTarget[servo2] = 163;
   }
   else
   {
     servoTarget[servo2] = 100;
   }
}
}