#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     lightSensor,    sensorLightInactive)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     LeftDrive,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     RightDrive,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     Base1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     Base2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     Top,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    LWrist,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    RWrist,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    Ramp,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Stop,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    AutoWrist,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    AutoArm,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void initializeRobot()
{

  servoTarget[LWrist] = 230;
  servoTarget[RWrist] = 0;
  servoTarget[AutoArm] = 0;
  servoTarget[AutoWrist] = 120;
  servoTarget[Stop] = 0;
  servoTarget[Ramp] = 245;
  wait1Msec(200);
}

task main()
{
   initializeRobot();

waitForStart();
{
 servo[AutoArm] = 140;
 wait1Msec(900);
}{
 servo[AutoArm] = 70;
 wait1Msec(900);
}{
 servo[AutoWrist] = 0;
 wait1Msec(900);
}{
 servo[AutoArm] = 10;
 wait1Msec(500);
}

nMotorEncoder[LeftDrive] = 0;
nMotorEncoder[RightDrive] = 0;

while (true)
{
    if ((nMotorEncoder[LeftDrive] + nMotorEncoder[RightDrive])/2 < 500)
    {
        motor[LeftDrive] = -50;
        motor[RightDrive] = 50;
    }
    else
    {
        motor[LeftDrive] = 0;
        motor[RightDrive] = 0;
    }
}

}

while (true)
{
    if ((nMotorEncoder[LeftDrive] + nMotorEncoder[RightDrive])/2 < 500)
    {
        motor[LeftDrive] = -50;
        motor[RightDrive] = 50;
    }
    else
    {
        motor[LeftDrive] = 0;
        motor[RightDrive] = 0;
    }
}