#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S3,     ,                    sensorTouch)
#pragma config(Motor,  motorA,          pulleymotor1,  tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          pulleymotor2,  tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Servo,  servo5,          ser1,                 tServoNormal)
#pragma config(Servo,  servo6,          ser2,                 tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const tSensors touchSensor = (tSensors)S3;

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

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


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void stopMotors()
{
	motor[motorD] = 0;
	motor[motorE] = 0;

	return;
}

void stopServos()
{
	servoTarget[ser1] = ServoValue[ser1];
	servoTarget[ser2] = ServoValue[ser2];

	return;
}

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  stopMotors();
  stopServos();


  return;
}




/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

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

  //variables
	int moveforwarddone = 0; //flag for once done moving forward
	long startTime; //time at start of loops
  int turnleftdone = 0; //flag for once done turning left
  int turnrightdone = 0; //flag for once done turning right
  int liftarmdone = 0; //flag for once done lifting arm
  int lowerarmdone = 0;  //flag for once done lowering arm
  int reversedone = 0;  //flag for once done reversing robot


  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////

  while (true)
  {




    startTime = nPgmTime;   //set the program time back to 0

    while(nPgmTime < startTime + 3000) //approximate distance from wall needed to turn
	  {
	    //while the sensor reads more than 13 cm. away from wall move forward full speed
		  motor[motorD] = 100;
		  motor[motorE] = -100;


		  moveforwarddone = 1;
    }

    //stop the motors
    stopMotors();



	  // turn robot left
	  if(moveforwarddone == 1)
	  {
	    startTime = nPgmTime;

		  while(nPgmTime < startTime + 500) // change to compass sensor -- need to get bearings once we get to competition location
		  {
			  motor[motorD] = 100;  //full speed with right wheel(make sure right wheel is motor A)
		    motor[motorE] = 100; //full reverse left wheel (make sure the left wheel is motor B)
	    }
	    turnleftdone = 1;
	    moveforwarddone = 0;   //reset moveforward done
    }

    //stop the motors
    stopMotors();

    //move robot towards the trough (3' 5.5") about 106 cm. NEED TO TEST PROBABLY HAVE TO RUN FOR TIME
    if(turnleftdone == 1)
    {
      startTime = nPgmTime;   //set the program time back to 0

      while(nPgmTime < startTime + 2000) //may need to lower or increase the amount of time for raising arms
  	  {
  	    motor[motorD] = 100;    //move forward maximum speed
        motor[motorE] = -100;
      }
      turnleftdone = 0;   //reset turnleftdone
      moveforwarddone = 1; //move forward done == 1 after bot reaches required position
    }

    //stop the motors
    stopMotors();

    //turn back to original position
    if(moveforwarddone == 1)
	  {
	    startTime = nPgmTime;   //set the program time back to 0

		  while(nPgmTime < startTime + 250) // run while heading != original heading (forward)
		  {
			  motor[motorD] = -100;  //full reverse with right wheel
		    motor[motorE] = -100; //full speed with left wheel
	    }
	    turnrightdone = 1;
	    moveforwarddone = 0;   //reset moveforward done
    }

    //stop the motors
    stopMotors();

    //lift arm to necessary hight (20.75") raise for time
    if(turnrightdone == 1)
    {
      startTime = nPgmTime;
      while(nPgmTime < startTime + 2000) // make pulleymotors lift to assist servos
      {
        motor[pulleymotor1] = 100;
        motor[pulleymotor2] = 100;
      }

  	  servo[ser1] = 80;    //make both servos lift the arm at maximum speed
      servo[ser2] = 175;



      liftarmdone = 1;
      moveforwarddone = 0; //reset move forward done
    }

    //stop servos
    stopServos();

    //move robot towards the wall to get arm close enough
    if(liftarmdone == 1)
    {
      startTime = nPgmTime;   //set the program time back to 0

      while(nPgmTime < startTime + 250) //approximate distance from wall needed to turn
	    {
	      //while the sensor reads more than 13 cm. away from wall move forward full speed
		    motor[motorD] = 100;
		    motor[motorE] = -100;
      }
      liftarmdone = 0; //reset lift arm
		  moveforwarddone = 1;

    }

    //stop the motors
    stopMotors();


    //lower arm if near a wall
    if(moveforwarddone == 1)
    {

      startTime = nPgmTime;
      while(nPgmTime < startTime + 2000) // make pulleymotors lower
      {
        motor[pulleymotor1] = -100;
        motor[pulleymotor2] = -100;
      }

  	  servo[ser1] = 0;    //make both servos lower the arm at maximum speed
      servo[ser2] = 255;


      lowerarmdone = 1;
      moveforwarddone = 0; //reset move forward done
    }

    //stop servos
    stopServos();

    //turn robot so that back faces straight towards the scoring area
    if(lowerarmdone == 1)
    {
      startTime = nPgmTime;   //set the program time back to 0

      while(nPgmTime < startTime + 150) // run while heading != original heading (about 20 degrees)
		  {
			  motor[motorD] = -100;  //full reverse with right wheel
		    motor[motorE] = -100; //full speed with left wheel
	    }

	    turnleftdone = 1;
	    lowerarmdone = 0;  //reset lower arm done
    }

    //stop the motors
    stopMotors();

    //reverse robot to scoring area
    if(turnleftdone == 1)
    {
      if(SensorValue[touchSensor] < 100 ) // run while touch sensor is not pushed in
		  {
		  	motor[motorD] = -100;  //full reverse with right wheel
		    motor[motorE] = 100; //full reverse with left wheel
	    }

	    turnleftdone = 0; //reset lower are done
	    reversedone = 1;
    }

    //stop the motors
    stopMotors();

    //score pucks until time runs out
    if(reversedone == 1)
    {
      startTime = nPgmTime;
      while(nPgmTime < startTime + 2000) // make pulleymotors lift to assist servos
      {
        motor[pulleymotor1] = 100;
        motor[pulleymotor2] = 100;
      }

      servo[ser1] = 110; //raise arm at full speed
      servo[ser2] = 145;
    }

    //stop servos
    stopServos();
  }
}