#pragma config(Hubs,  S1, HTServo,  HTServo,  HTMotor,  HTMotor)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C3_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C1_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_6,    servo6,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_1,    servo7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo10,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo11,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo12,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

void initializeRobot()
{
  //from the front of the robot
  servo[servo1] = 138; //right bowling ball servo up pos
  servo[servo12] = 96; //left bowling ball servo up pos

  servo[servo7] = 0; //from the front, the left hand servo is a 0 when the arm is extended. It is at 229 when it is at its lowest.
  servo[servo8] = 0; //from the front, the right hand servo is a 26 when the arm is extended. It is at 255 when it is at its lowest.

  servo[servo9] = 225; //small servo joint
  servo[servo2] = 0;

  servo[servo11] = 225;
  return;
}

task main()
{
  nNoMessageCounterLimit = 200;
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

 //Going forwards

  motor[motorD] = 40;
  motor[motorE] = -40;
  wait1Msec(2000);

  motor[motorD] = 0;
  motor[motorE] = 0;
  wait1Msec(500);

  //Going back

  motor[motorD] = -40;
  motor[motorE] = 40;
  wait1Msec(250);

  motor[motorD] = 0;
  motor[motorE] = 0;
  wait1Msec(500);

  //Turning

  motor[motorD] = 50;
  motor[motorE] = 50;
  wait1Msec(800);

  motor[motorD] = 0;
  motor[motorE] = 0;
  wait1Msec(500);

  //Going forward after turning

  motor[motorD] = -40;
  motor[motorE] = 40;
  wait1Msec(5000);

  motor[motorD] = 0;
  motor[motorE] = 0;
  wait1Msec(500);


  while (true)
  {}