#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C1_1,     motorF,        tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S2_C1_2,     motorG,        tmotorTetrix, PIDControl, reversed)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"




void initializeRobot()
{
   servo[servo1]=255;
   servo[servo2]=0;
   servo[servo3]=128;
   servo[servo4]=128;
   servo[servo5]=255;
    nMotorEncoder[motorD] = 0;
  nMotorEncoder[motorE] = 0;
   return;
}




task main()
{
   initializeRobot();

     servo[servo1]=255;
   servo[servo2]=0;
   servo[servo3]=128;
   servo[servo4]=128;
   servo[servo5]=110;
   motor[motorF]=0;
   motor[motorG]=0;
nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;

  nMotorEncoderTarget[motorE] = 0;
  nMotorEncoderTarget[motorD] = 462;

  motor[motorE] = 0;
  motor[motorD] = 50;

  while (nMotorRunState[motorE] != runStateIdle || nMotorRunState[motorD] != runStateIdle)
  {

  }

  motor[motorE] = 0;
  motor[motorD] = 0;

  wait1Msec(3000);

        servo[servo1]=255;
   servo[servo2]=0;
   servo[servo3]=128;
   servo[servo4]=128;
   servo[servo5]=110;
   motor[motorF]=0;
   motor[motorG]=0;
nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;

  nMotorEncoderTarget[motorE] = 7312;
  nMotorEncoderTarget[motorD] = 7312;

  motor[motorE] = 50;
  motor[motorD] = 50;

  while (nMotorRunState[motorE] != runStateIdle || nMotorRunState[motorD] != runStateIdle)
  {

  }

  motor[motorE] = 0;
  motor[motorD] = 0;

  wait1Msec(3000);

        servo[servo1]=255;
   servo[servo2]=0;
   servo[servo3]=128;
   servo[servo4]=128;
   servo[servo5]=110;
   motor[motorF]=0;
   motor[motorG]=0;

nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;

  nMotorEncoderTarget[motorE] = 462;
  nMotorEncoderTarget[motorD] = 0;

  motor[motorE] = 50;
  motor[motorD] = 1;

  while (nMotorRunState[motorE] != runStateIdle || nMotorRunState[motorD] != runStateIdle)
  {

  }

  motor[motorE] = 0;
  motor[motorD] = 0;

  wait1Msec(3000);