#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight1,   tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight2,   tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLeft1,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorLeft2,    tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    rightServo,           tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    leftServo,            tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"

const int JOYSTICKPADDING = 17;
const float JOYSTICKRATIO = .7813;

int leftPower, rightPower;

void setMotors(int leftMotorPower, int rightMotorPower){
  motor[motorLeft1] = leftMotorPower;
  motor[motorLeft2] = leftMotorPower;

  motor[motorRight1] = rightMotorPower;
  motor[motorRight2] = rightMotorPower;
}

void setServos(int leftServoAngle, int rightServoAngle){
  servo[leftServo] = leftServoAngle;
  servo[rightServo] = rightServoAngle;
}

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

  while (true)
  {


    getJoystickSettings(joystick); // Update Joystick variables


    leftPower = (int)(joystick.joy1_y1 * JOYSTICKRATIO);
    rightPower = (int)(joystick.joy1_y2 * JOYSTICKRATIO);

    if (abs(leftPower) < JOYSTICKPADDING) {
      leftPower = 0;
    }

    if (abs(rightPower) < JOYSTICKPADDING) {
      rightPower = 0;
    }

    setMotors(leftPower, rightPower);


    if(joy1Btn(5)){
      setServos(0, 260);
    }else if(joy1Btn(6)){
      setServos(80,175);
    }
  }
}