#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1, servo1, tServoNormal)
#pragma config(Servo,  srvo_S1_C2_2, servo2, tServoNormal)
#pragma config(Servo,  srvo_S1_C2_3, servo3, tServoNormal)

#include "JoystickDriver.c"
void swivelArm();
void joint1up();
void joint1down();
void joint2up();
void joint2down();
void joint3open();
void joint3close();
int servo1Pos = ServoValue(servo1);
int servo2Pos = ServoValue(servo2);
int servo3Pos = ServoValue(servo3);

task main()
{
  while(true)
  {
    getJoystickSettings(joystick);

    swivelArm();

    if(joy1Btn(5))
    {
   joint1up();
   wait1Msec(10);
   }
    if(joy1Btn(6))
    {
   joint1down();
   wait1Msec(10);
    }
  if(joy1Btn(7))
    {
   joint2up();
   wait1Msec(10);
   }
    if(joy1Btn(8))
    {
   joint2down();
   wait1Msec(10);
    }
  if(joy1Btn(9))
    {
   joint3open();
   wait1Msec(10);
   }
    if(joy1Btn(10))
    {
   joint3close();
   wait1Msec(10);
    }
  }
}

void swivelArm()
{
  motor[motorD] = joystick.joy1_x1/20;
  }

void joint1up()
{
   if(servo1Pos < 100)
   {
      servo[servo1] = servo1Pos++; 
   }
}

void joint1down()
{
    if(servo1Pos > 0)
    {
       servo[servo1] = servo1Pos--; 
    }
}
void joint2up()
{
   if(servo2Pos > 1)
   {
      servo[servo2] = servo2Pos--; 
   }
}

void joint2down()
{
    if(servo2Pos < 187)
    {
       servo[servo2] = servo2Pos++; 
    }
}

void joint3open()
{
   if(servo3Pos > 1)
   {
      servo[servo3] = servo3Pos--; 
   }
}

void joint3close()
{
    if(servo3Pos < 100)
    {
       servo[servo3] = servo3Pos++; 
  }
}

#if defined(NXT) || defined(TETRIX)
   #pragma autoStartTasks        // Automatically start this task when the main user program starts.
#elif defined(VEX)
   #pragma platform(VEX, FRC)
#endif