#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     motor_belt,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motor_left,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motor_right,   tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    right_arm_x,          tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    right_arm_y,          tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    left_arm_y,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    left_arm_x,           tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// TURNING
// 90 degree turn = 1720
// 180 degree turn = 3450... yeah, I don't know either.

// SERVOS
// Servo 1: right_arm_x
// Servo 2: right_arm_y
// Servo 5: left_arm_y
// Servo 6: left_arm_x


task main()

{


 
//Go down ramp.

  nMotorEncoder[motor_left] = 0;
  nMotorEncoder[motor_right] = 0;

  wait1Msec(100);
 
  nMotorEncoderTarget[motor_left] = 5600;
  nMotorEncoderTarget[motor_right] = 5600;


  motor[motor_left] = -75;
  motor[motor_right] = -75;

  while(nMotorRunState[motor_right] != runStateIdle)
   {

   }

  motor[motor_left] = 0;
  motor[motor_right] = 0;


// Turn right.

  nMotorEncoder[motor_left] = 0;
  nMotorEncoder[motor_right] = 0;

  wait1Msec(100);
 
  nMotorEncoderTarget[motor_left] = 2150;
  nMotorEncoderTarget[motor_right] = 2150;


  motor[motor_left] = 75;
  motor[motor_right] = -75;

  while(nMotorRunState[motor_right] != runStateIdle)
   {

   }

  motor[motor_left] = 0;
  motor[motor_right] = 0;

 
   
// Push bowling ball.
 
  nMotorEncoder[motor_left] = 0;
  nMotorEncoder[motor_right] = 0;

  wait1Msec(100);
 
  nMotorEncoderTarget[motor_left] = 8000;
  nMotorEncoderTarget[motor_right] = 8000;


  motor[motor_left] = -25;
  motor[motor_right] = -25;

  while(nMotorRunState[motor_right] != runStateIdle)
   {

   }

  motor[motor_left] = 0;
  motor[motor_right] = 0;


}

// Turn right.

  nMotorEncoder[motor_left] = 0;
  nMotorEncoder[motor_right] = 0;

  wait1Msec(100);
 
  nMotorEncoderTarget[motor_left] = 2150;
  nMotorEncoderTarget[motor_right] = 2150;


  motor[motor_left] = 75;
  motor[motor_right] = -75;

  while(nMotorRunState[motor_right] != runStateIdle)
   {

   }

  motor[motor_left] = 0;
  motor[motor_right] = 0;