#pragma config(Motor,  motor1,          leftMotor,     tmotorVexIQ, openLoop, encoder)
#pragma config(Motor,  motor6,          rightMotor,    tmotorVexIQ, openLoop, reversed, encoder)
#pragma config(Motor,  motor10,         armMotor,      tmotorVexIQ, openLoop, encoder)
#pragma config(Motor,  motor11,         clawMotor,     tmotorVexIQ, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*------------------------------------------------------------------------------------------------
  This program will use the VEX IQ Wirless Controller to drive your Clawbot. This program uses
  If/Else statements to provide a "threshold" for the transmitter - this allows your robot to
  come to a stop even if the joysticks on the Wireless Controller haven't perfectly returned to
  their zero position.

  Note: You will have to set ROBOTC to enable "Wireless Control" to use the VEX IQ joystick.

                            ROBOT CONFIGURATION: VEX IQ Clawbot
    MOTORS & SENSORS:
    [I/O Port]          [Name]              [Type]                         [Location]
    Port 1                 leftMotor           VEX IQ Motor                     Left side motor
    Port 6                   rightMotor          VEX IQ Motor                     Right side motor (reversed)
      Port 10                     armMotor                  VEX IQ Motor                        Arm Up/Down motor
      Port 11                     clawMotor                  VEX IQ Motor                        Claw Open/Close motor
------------------------------------------------------------------------------------------------*/

task main()
{
   int threshold = 10;
   static int leftSpeed = 0;
   static int rightSpeed = 0;

   while(true)
   {
      leftSpeed = (getJoystickValue(ChA) - getJoystickValue(ChB)/2);   // (y - x)/2
      rightSpeed = (getJoystickValue(ChA) + getJoystickValue(ChB)/2);   // (y + x)/2

      if(leftSpeed > threshold || leftSpeed < -threshold)
      {
         setMotorSpeed(leftMotor, leftSpeed);
      }
      else
      {
         setMotorSpeed(leftMotor, 0);
      }

      if(rightSpeed > threshold || rightSpeed < -threshold)
      {
         setMotorSpeed(rightMotor, rightSpeed);
      }
      else
      {
         setMotorSpeed(rightMotor, 0);
      }

      //If Button "L-Up" is pressed in, we'll set the arm motor to run in reverse.
      if(getJoystickValue(BtnLUp) == 1)
      {
         setMotorSpeed(armMotor, -127);
      }
      //If the "L-Up" isn't pressed, but "L-Down" is, we'll set the motor to run forward.
      else if(getJoystickValue(BtnLDown) == 1)
      {
         setMotorSpeed(armMotor, 127);
      }
      else  //If neither button is pressed, we'll set the motor off.
      {
         setMotorSpeed(armMotor, 0);
      }

      //If Button "L-Up" is pressed in, we'll set the arm motor to run in reverse.
      if(getJoystickValue(BtnRUp) == 1)
      {
         setMotorSpeed(clawMotor, -127);
      }
      //If the "L-Up" isn't pressed, but "L-Down" is, we'll set the motor to run forward.
      else if(getJoystickValue(BtnRDown) == 1)
      {
         setMotorSpeed(clawMotor, 127);
      }
      else   //If neither button is pressed, we'll set the motor off.
      {
         setMotorSpeed(clawMotor, 0);
      }
   }
}

if(getJoystickValue(BtnRUp) == 1)
      {
         setMotorSpeed(clawMotor, -127);
      }