#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTServo,  HTServo,  HTServo)
#pragma config(Motor,  mtr_S1_C1_1,     rightMotor,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftMotor,     tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     shoulderMotor, tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    claw,                 tServoNormal)
#pragma config(Servo,  srvo_S2_C1_2,    wrist,                tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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


void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


task main()
{
  initializeRobot();

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

  int thresholdP = 10;
  int thresholdN = -10;

  while (true)
  {
    getJoystickSettings(joystick);
    if((joystick.joy1_y1) > thresholdP)
    {
      motor[rightMotor] = (((joystick.joy1_y1) * (joystick.joy1_y1)) / 161.29);
    }
    if((joystick.joy1_y1) < thresholdN)
    {
      motor[rightMotor] = (((joystick.joy1_y1) * (joystick.joy1_y1)) / -161.29);
    }
    if((joystick.joy1_y2) > thresholdP);
    {
      motor[rightMotor] = (((joystick.joy1_y2) * (joystick.joy1_y2)) / 161.29);
    }
    if((joystick.joy1_y2) < thresholdN);
    {
      motor[rightMotor] = (((joystick.joy1_y2) * (joystick.joy1_y2)) / -161.29);
    }
    if((joystick.joy2_y1) > thresholdP);
    {
      motor[shoulderMotor] = (((joystick.joy2_y1) * (joystick.joy2_y1)) / 161.29);
    }
    if((joystick.joy2_y1) < thresholdN);
    {
      motor[shoulderMotor] = (((joystick.joy2_y1) * (joystick.joy2_y1)) / -161.29);
    }
    if((joystick.joy2_y2) > thresholdP);
    {
      motor[motorA] = (((joystick.joy2_y1) * (joystick.joy2_y1)) / 161.29);
    }
    if((joystick.joy2_y2) > thresholdN);
    {
      motor[motorA] = (((joystick.joy2_y1) * (joystick.joy2_y1)) / -161.29);
    }
    if((joy2Btn(5) == 1));
    {
      servo(wrist) = 90;
    }
    if((joy2Btn(6) == 1));
    {
      servo(wrist) = 180;
    }
    if((joy2Btn(7) == 1));
    {
      servo(claw) = 90;
    }
    if((joy2Btn(8) == 1));
    {
      servo(claw) = 180;
    }
  }
}