3rdDesign

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    ExternalBatteryValue, sensorAnalog)
#pragma config(Sensor, in2,    GyroPosition,   sensorGyro)
#pragma config(Sensor, in3,    liftPosition,   sensorPotentiometer)
#pragma config(Sensor, in4,    lineRight,      sensorLineFollower)
#pragma config(Sensor, in5,    lineLeft,       sensorLineFollower)
#pragma config(Sensor, in6,    clawLiftPosition, sensorPotentiometer)
#pragma config(Sensor, in7,    mobileLiftPosition, sensorPotentiometer)
#pragma config(Sensor, in8,    lineBack,       sensorLineFollower)
#pragma config(Sensor, dgtl1,  clawLiftUp,     sensorTouch)
#pragma config(Sensor, dgtl3,  clawLiftDown,   sensorNone)
#pragma config(Sensor, dgtl4,  liftDown,       sensorTouch)
#pragma config(Sensor, dgtl5,  mobileLiftDown, sensorTouch)
#pragma config(Sensor, dgtl6,  stationaryGoal, sensorTouch)
#pragma config(Sensor, dgtl11, coneHeight,     sensorNone)
#pragma config(Sensor, dgtl12, clawLiftPosition, sensorNone)
#pragma config(Sensor, I2C_1,  RearRight,      sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           clawMotor,     tmotorVex393HighSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           wheelRight,    tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port3,           wheelLeft,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           clawLiftL,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           clawLiftR,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           wheelLeftMid,  tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           armLiftLeftB,  tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           armLiftRightB, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           wheelRightMid, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          mobileLift,    tmotorVex393HighSpeed_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
// #pragma config(Motor,  port6,           armLiftLeftT,  tmotorVex393HighSpeed_MC29, openLoop, reversed)
// #pragma config(Motor,  port9,           armLiftRightT, tmotorVex393HighSpeed_MC29, openLoop)

//COLOR CODING!
//clawMotor = puke
//wheelLeftFront = skyblue
//wheelLeftBack = oceanblue
//wheelRightFront = palepink
//wheelRightBack = flamingopink
//clawLiftL = neongreen
//clawLiftR = darkgreen
//armLiftLeftB
/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/
//When the potentiometer value is changed, make sure you check the actual
//sensor values and change the program values accordingly
//7D is MinLift, 7R LiftPickMobile, 8D FeedLift
// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"

const int MAX_CLAWLIFT = 3000;
const int MIN_CLAWLIFT = 900; //2100 + 300;
const int MID_CLAWLIFT = (MAX_CLAWLIFT + MIN_CLAWLIFT) / 2;
//const int CLAWLIFT_OFFSET = 300;
const int ERROR_CLAWLIFT = 300;
const int MAX_MOBILELIFT = 2900;
const int STOP_MOBILELIFT = 2000;
// const int HIGHGOAL_MOBILELIFT = 2800;
const int DRIVING_MOBILELIFT = 2700 - 600;
// const int HIGHGOAL_MOBILELIFT = 2800;


// const int MAX_LIFT = 1600;
const int MAX_LIFT = 2000;
const int MIN_LIFT = 500; //620
//const int AUTOSTART_LIFT = 1800 ; //620
const int SLOWDOWN_LIFT = 3200;
//const int PICKUP_LIFT = 2000 ; //620

const int FEED_LIFT = 2450 - 150;
//const int MID_LIFT = (MAX_LIFT + MIN_LIFT) / 3;
const int LIFT_PICK_MOBILE = 800;
const int STATIONARY_LIFT = 2500;

const int GLOBAL_WAITER = 100;
const int GLOBAL_WAITER_LONG = 100;

const int COLOR_DIFF = 30 - 7; //comp value: 50
int COLOR_BASE_BACK = 3000;
int COLOR_BASE_RIGHT = 3000;
int COLOR_BASE_LEFT = 3000;

typedef enum {
	Right = 1,
	Left = 2,
	Back = 3
} LineType;

// typedef struct{
// 	LineTypeStruct Type;
// } LineType;

// LineTypeStruct LineType = Back;

int autonomousMode = 1;
const int drivingSkillMode = 7;
// 6: ProgSkill
// 7: DrvSkill


const int FULLPOWER = 127;
//const int HALFFULLPOWER = 64;

//const int powerliftUp = 120;
const int powerliftDown = 100;//50;
//const int powerliftDownPickUp = powerliftDown * 2;
const int powerClawLiftUp = 90 - 10;
const int powerClawLiftDown = 100;
const int powerClawOpen = 60;
const int powerClawClose = 100;
const int powerClawHold = 60;

string LIFT_STATUS = "";

int CONE_COUNT = 0;
// int CONE_COUNT_STATIONARY = 0;
// int LIFT_TARGET = MIN_LIFT;
// int STATIONARY_LIFT_TARGET = 0;
// int STATIONARY_GOAL_TARGET = 0;
int STATIONARY_GOAL_TARGET_NO_MG = -1;

// int GLOGAL_MOBILELIFTPOSITION = 0;

// ---------------------------------------
// TASKS
task move();
task lift();
task claw();
task clawLift();
// END TASKS


// ---------------------------------------
// FUNCTIONS
// LIFT
void moveLift(int power);
void moveLiftUp(int power);
void moveLiftDown(int power);
void holdLift();
void holdLiftAuton();
void holdDownLift();

bool startAuton(int power);

bool moveLiftUpAndStop(int distance, int power);
bool moveLiftUpAndHold(int distance, int power);
bool moveLiftDownAndStop(int distance, int power);
bool moveLiftDownAndHold(int distance, int power);
bool moveLiftUpAndHoldAuton(int distance, int power);
bool moveLiftDownAndHold_Touch(int power);
bool moveLiftDownAndStop_Touch(int power);
bool moveLiftUpAndHold_Smart(int distance, int power);

// CLAW LIFT
void moveClawLift(int power);
void moveClawLiftUp(int power);
void moveClawLiftDown(int power);
bool moveClawLiftUpAndStop(int distance, int power);
bool moveClawLiftDownAndStop(int distance, int power);
bool moveClawLiftDownAndHold(int distance, int power);
bool moveClawLiftDownAndHelper(int distance, int power);

// CLAW
void moveClaw(int power);
void openClaw(int power);
void closeClaw(int power);

// MOBILE LIFT
void moveMobileLift(int power);
void moveMobileLiftUp(int power);
void pushMobileLiftUp();
//void moveMobileLiftUpAuton();
void moveMobileLiftDown(int power);
void moveMobileLiftUpAndStop();
void moveMobileLiftUpAndStopDrivingSkill();
void moveMobileLiftDownAndStop(bool higher);
void moveMobileLiftDownAndStopAuton(int distance);
void moveMobileLiftDownToPickUpAuton();
void stopMobileLift();

// MOVE
//void MoveAccel(int distance, int power);
//void MoveAccelAuton(int distance, int power);
//void MoveAccelMobile(int distance, int power);
void MoveIEM(int distance, int power);
//void MoveForward(int distance);
//void MoveBackword(int distance);
void MoveHelper(int power);
void TurnGyro(int distance, int power);
void TurnGyroLeft(float angle);
void TurnGyroRight(float angle);
void TurnHelper(int power);

// MACRO
void moveMobileLiftUpToStackAuton(int powerLift);
void moveMobileLiftDownToPickUp();
void scoreStationaryGoal();
void moveLiftUpForStationaryGoal(int liftValue);
void moveLiftUpForStationaryGoalAuton(int liftValue);


bool LiftDownPickUpClawLiftUpLiftDown_PART1(int target, bool manualFeed);
//void LiftDownPickUpClawLiftUpLiftDown_PART1_2();
bool LiftDownPickUpClawLiftUpLiftDown_PART2(int target, bool manualFeed);
void PickUpAndMoveLiftUp();
bool PushORUpCone(bool pushDown);
void PushAndUpCone();

// BATTERY
int AdjustPowerUsingExternalBatteryLevelToMove(int originalPower);

// LCD
void AutonomousSelector(int btnLCD);
void DisplayAutonomousMode(int autonomousModeValue);
void displayBatteryLevelOnLCD(int autonomousModeValue);

// SENSORS
int getLiftDown();
int getLiftPosition();
int getMobileLiftPosition();
//int getNewLiftTargetPosition(bool manualFeed, bool increase);
//int getCurrentLiftTargetPosition(bool manualFeed);
int getLiftTargetPosition();
//int getTargetLiftTargetPosition1(bool manualFeed);
int getClawLiftPosition();
int getStaionaryGoal();
int getLiftTargetPositionForStationaryGoal();
int getLineRight();
int getLineLeft();
int getLineBack();
// void moveLiftUpAndRelease(int distanceLift, int powerLift, int distanceClawLift, int powerClawLift);

// HELPERS
bool autoCommon(int coneCount, bool leftSide);
void SetInitialPositionAuton();

void autonCone20(int coneCount, bool leftSide);
void autonCone5(bool leftSide);
void autonStationary(bool leftSide);
void defensive(bool leftSide);
void programmingSkill();

void HitBarAndAlign();

void stopAll();
void stopLift();
void stopClawLift();
void stopClaw();

// END FUNCTIONS

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks
	// running between Autonomous and Driver controlled modes. You will need to
	// manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	SensorValue[lineBack] = sensorNone;
	SensorValue[lineRight] = sensorNone;
	SensorValue[lineLeft] = sensorNone;
	SensorValue[GyroPosition] = sensorNone;
	wait1Msec(1000);


	SensorValue[GyroPosition] = sensorGyro;
	SensorValue[lineBack] = sensorLineFollower;
	SensorValue[lineRight] = sensorLineFollower;
	SensorValue[lineLeft] = sensorLineFollower;
	wait1Msec(1000);

	SensorFullCount[GyroPosition] = 3600;
	SensorValue[GyroPosition] = 0;
	nMotorEncoder[wheelRight] = 0;

	COLOR_BASE_BACK = SensorValue[lineBack];
	COLOR_BASE_RIGHT = SensorValue[lineRight];
	COLOR_BASE_LEFT = SensorValue[lineLeft];


	writeDebugStreamLine("");
	writeDebugStreamLine("-------------------------- Starts --------------------------");
	writeDebugStreamLine("");
	// startAuton(FULLPOWER);

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...

}

bool startAuton(int power) {

	clearTimer(T2);
	int tcur = getLiftPosition();
	if (tcur > MIN_LIFT){
		return true;
	}

	moveLift(-power);
	wait1Msec(200);

	while (getLiftDown() == 0){
		// writeDebugStreamLine("in");
		// wait1Msec(50);

		moveLift(-power);
		if (vexRT[Btn8L] == 1 || time1[T2] > 500)
		{
			writeDebugStreamLine("timeout");

			stopLift();
			return false;
		}
	}

	// writeDebugStreamLine("out");

	stopLift();

	return true;
}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{

	long startTime = nPgmTime;

	//startAuton(FULLPOWER);

	if (autonomousMode == 1)
	{
		// TurnGyroLeft(180);

		autonCone20(2, true);
	}
	else if (autonomousMode == 2)
	{
		autonCone20(2, false);
	}
	else if (autonomousMode == 3)
	{
		autonCone5(true);
	}
	else if (autonomousMode == 4)
	{
		autonCone5(false);
	}
	else if (autonomousMode == 5)
	{
		autonStationary(true);
	}
	else if (autonomousMode == 6)
	{
		autonStationary(false);
	}
	else if (autonomousMode == 7)
	{
		defensive(true);
	}
	else if (autonomousMode == 8)
	{
		defensive(false);
	}
	else if (autonomousMode == 9)
	{
		programmingSkill();
	}


	writeDebugStreamLine("autonomous) Time: %d", nPgmTime - startTime);

	// ..........................................................................
	// Insert user code here.
	// ..........................................................................

	// Remove this function call once you have "real" code.

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/


// task updateSensors()
// {
// 	while (true)
// 	{
// 		wait1Msec(50);
// 		GLOGAL_MOBILELIFTPOSITION = SensorValue(mobileLiftPosition);
// 	}
// }

task move()
{
	while (true)
	{
		wait1Msec(50);

		int ch1 = vexRT[Ch1];
		int ch3 = vexRT[Ch3];

		if (abs(ch1) > 17)
		{

			if (autonomousMode == drivingSkillMode)
			{
				ch1 = ch1 * 3 / 5;
				//ch3 = ch3 * 2 / 3; //fastest pair was 3/5 for turning and 4/5 for going back and forth
			}

			TurnHelper(ch1);
		}
		else if (abs(ch3) > 17)
		{
			MoveHelper(ch3);
		}
		else
		{
			MoveHelper(0);
		}
		// motor[wheelLeft] = motor[wheelLeftMid] = -ch3 - ch1;
		// motor[wheelRight] = motor[wheelRightMid] = ch3 - ch1;

		// if (abs(motor[wheelRight]) < 17 || abs(motor[wheelLeft]) < 17)
		// {
		// 	motor[wheelLeft] = motor[wheelLeftMid] = 0;
		// 	motor[wheelRight] = motor[wheelRightMid] = 0;
		// }
	}
}




task lift()
{
	while (true)
	{
		wait1Msec(50);

		if (vexRT[Btn8R] == 1) {
			if (vexRT[Btn7U] == 1) { //Move Lift up with btn 7U
				LIFT_STATUS = "manual";
				moveLiftUp(FULLPOWER); //80
			}

			else if (vexRT[Btn7D] == 1){ //Move Lift down with btn 7D
				LIFT_STATUS = "manual";
				// moveLiftDownAndHold_Touch(powerliftDown); //-40
				moveLiftDown(FULLPOWER); //-40

			}
			else if (vexRT[Btn7R] == 1){
				moveMobileLiftDownAndStop(false);
				// moveMobileLiftDownToPickUp();
			}
			else if (vexRT[Btn7L] == 1){
				moveMobileLiftUpAndStop();
			}
			else if (vexRT[Btn6U] == 1){
				scoreStationaryGoal();
			}
			else if (vexRT[Btn6D] == 1){
				moveLiftUpForStationaryGoal(STATIONARY_LIFT);
			}


			// if (vexRT[Btn5U] == 0 && vexRT[Btn5D] == 0){
			// 	stopClawLift();
			// }
			if (vexRT[Btn6U] == 0) {
				stopMobileLift();
			}

			if (LIFT_STATUS == "manual" && vexRT[Btn7U] == 0 && vexRT[Btn7D] == 0){
				stopLift();
			}

			if (vexRT[Btn7R] == 0 && vexRT[Btn7L] == 0){
				stopMobileLift();
			}
		}
		else if (vexRT[Btn8R] == 0) {

			if (vexRT[Btn7U] == 1) {

				// writeDebugStreamLine("7U");

				CONE_COUNT++;
				int liftTarget = getLiftTargetPosition();
				liftTarget = liftTarget + MIN_LIFT;
				int clawLiftPosition = getClawLiftPosition();

				if (clawLiftPosition >= MID_CLAWLIFT){
					LiftDownPickUpClawLiftUpLiftDown_PART1(liftTarget, false);
					LiftDownPickUpClawLiftUpLiftDown_PART2(liftTarget, false);
				}
				else if (clawLiftPosition < MID_CLAWLIFT)
				{
					LiftDownPickUpClawLiftUpLiftDown_PART2(liftTarget, false);
				}
			}
			else if (vexRT[Btn7D] == 1) {

				// writeDebugStreamLine("7D");

				int liftTarget = getLiftTargetPosition();
				liftTarget = liftTarget + MIN_LIFT;

				int clawLiftPosition = getClawLiftPosition();

				if (clawLiftPosition >= MID_CLAWLIFT){
					LiftDownPickUpClawLiftUpLiftDown_PART1(liftTarget, false);
					LiftDownPickUpClawLiftUpLiftDown_PART2(liftTarget, false);
				}
				else if (clawLiftPosition < MID_CLAWLIFT)
				{
					LiftDownPickUpClawLiftUpLiftDown_PART2(liftTarget, false);
				}
			}
			else if (vexRT[Btn7L] == 1){
				if (autonomousMode == drivingSkillMode){
					moveMobileLiftUpAndStopDrivingSkill();
				}
				else{
					moveMobileLiftUpAndStop();
				}

			}
			else if (vexRT[Btn7R] == 1){
				// writeDebugStreamLine("7R");

				moveMobileLiftDownToPickUp();
			}
			else if (vexRT[Btn8U] == 1){
				PickUpAndMoveLiftUp();
			}
			// Single button
			else if (vexRT[Btn8D] == 1){
				// writeDebugStreamLine("8D");

				if (false){
					moveClawLiftDown(powerClawLiftDown - 30);

					int clawLiftPosition = getClawLiftPosition();
					while (true){

						clawLiftPosition = getClawLiftPosition();
						if (clawLiftPosition < MIN_CLAWLIFT){
							break;
						}

						if (vexRT[Btn8L] == 1)
						{
							break;
						}


					}
					stopClawLift();
					wait1Msec(200);
				}
				else
				{
					int pClawLiftDown = powerClawLiftDown * 4 / 5;
					if (getLiftPosition() > 1400) {
						pClawLiftDown = powerClawLiftDown * 3 / 5;
					}

					if (getLiftPosition() < 900) {

						moveClawLiftDownAndHold(MIN_CLAWLIFT /*MID_CLAWLIFT /*- CLAWLIFT_OFFSET*/, /*FULLPOWER - 20*/pClawLiftDown);
					}
					else
					{
						moveClawLiftDown(pClawLiftDown);

						while (true){

							int clawLiftPosition = getClawLiftPosition();
							int liftPosition = getLiftPosition();
							if (clawLiftPosition < MIN_CLAWLIFT){
								break;
							}

							if (clawLiftPosition < MID_CLAWLIFT){
								writeDebugStreamLine("MID_LIFt started");
								moveLiftDown(FULLPOWER);
							}

							if (liftPosition < MIN_LIFT){
								writeDebugStreamLine("MID_LIFt stop");
								moveLiftDown(0);
							}


							if (vexRT[Btn8L] == 1)
							{
								break;
							}


						}
						moveClawLiftDown(10);

					}

					// wait1Msec(100);
				}

				closeClaw(FULLPOWER);
				moveLiftDown(FULLPOWER);

				while (true){

					if (vexRT[Btn8L] == 1)
					{
						stopClaw();
						break;
					}
					else if (vexRT[Btn8U] == 1 || vexRT[Btn5U] == 1 || vexRT[Btn5D] == 1 || getLiftDown() == 1)
					{
						break;
					}
				}

				holdDownLift();
			}

			if (vexRT[Btn7R] == 0 && vexRT[Btn7L] == 0){
				stopMobileLift();
			}
		}
	}
}

void PickUpAndMoveLiftUp()
{
	pushMobileLiftUp();
	closeClaw(FULLPOWER);
	wait1Msec(100);
	closeClaw(powerClawHold);

	// writeDebugStreamLine("1 ClawPower: %d", motor[clawMotor]);
	moveLiftUp(FULLPOWER);

	// writeDebugStreamLine("2 ClawPower: %d", motor[clawMotor]);

	while (true){

		// writeDebugStreamLine("3 ClawPower: %d", motor[clawMotor]);


		int liftPosition = getLiftPosition();

		if (liftPosition > MAX_LIFT + 50){
			holdLiftAuton();
		}

		if (vexRT[Btn8L] == 1)
		{
			// stopClaw();
			break;
		}
		else if (vexRT[Btn5U] == 1 || vexRT[Btn5D] == 1 || vexRT[Btn8D] == 1)
		{
			break;
		}
	}


	// writeDebugStreamLine("4 ClawPower: %d", motor[clawMotor]);

	// holdLift();
}

bool LiftDownPickUpClawLiftUpLiftDown_PART1(int target, bool manualFeed)
{
	// writeDebugStreamLine("PART1> Target: %d, Cone: %d", target, CONE_COUNT);

	LIFT_STATUS = "LiftDownPickUpClawLiftUpLiftDown_PART1";
	// READY PICKUP
	openClaw(powerClawOpen);
	wait1Msec(400);

	bool ret = true;

	ret = PushORUpCone(false);
	if (CONE_COUNT >= 11)
	{
		wait1Msec(100);
	}
	// writeDebugStreamLine("201");

	stopClaw();



	// ret = moveClawLiftDownAndStop(MIN_CLAWLIFT /*MID_CLAWLIFT - CLAWLIFT_OFFSET*/, powerClawLiftDown /*powerClawLiftDown*/);
	ret = moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown /*powerClawLiftDown*/);
	if (!ret){
		return false;
	}
	LIFT_STATUS = "";


	// writeDebugStreamLine("PART1> clawLiftPosition: %d", getClawLiftPosition());


	// writeDebugStreamLine("202");

	wait1Msec(100);

	return true;
}

bool LiftDownPickUpClawLiftUpLiftDown_PART2(int target, bool manualFeed)
{
	// writeDebugStreamLine("PART2> Target: %d, Cone: %d, clawLiftPosition: %d", target, CONE_COUNT, getClawLiftPosition());
	LIFT_STATUS = "LiftDownPickUpClawLiftUpLiftDown_PART2";

	// int target = getCurrentLiftTargetPosition(manualFeed, false);
	// int target = liftValue + offset;
	// LIFT_TARGET = target;
	bool ret = true;

	// PICKUP
	closeClaw(FULLPOWER);

	while(getClawLiftPosition() > MIN_CLAWLIFT){
		if (vexRT[Btn8L] == 1)
		{
			return false;
		}
	}

	// writeDebugStreamLine("PART2> clawLiftPosition: %d", getClawLiftPosition());

	if (manualFeed){
		int tTarget = FEED_LIFT;// - 100 + 100;
		int tPower = 80 + 20;

		tPower = FULLPOWER;

		ret = moveLiftDownAndStop(tTarget, tPower);
		if (!ret){
			return false;
		}
		wait1Msec(200);
	}
	else {
		int tPower = FULLPOWER;
		if (CONE_COUNT >= 10){
			tPower = 80;
		}
		else if (CONE_COUNT >= 8){
			tPower = 95;
		}

		tPower = FULLPOWER;
		ret = moveLiftDownAndHold_Touch(tPower);
		if (!ret){
			return false;
		}

		wait1Msec(200);
	}

	// // PICK UP CONE AND HOLD
	// wait1Msec(450 - 250);

	// CONE HOLD
	if (manualFeed) {
		closeClaw(powerClawHold + 10);
	}
	else {
		closeClaw(powerClawHold);
	}


	if (manualFeed){
		if (CONE_COUNT >= 3){
			ret = moveLiftUpAndHold_Smart(target + 50, FULLPOWER);
			if (!ret){
				writeDebugStreamLine("!ret");
				return false;
			}
		}

		wait1Msec(100);


		// writeDebugStreamLine("100 - 4");

	}
	else {

		// writeDebugStreamLine("102");
		ret = moveLiftUpAndHold_Smart(target, FULLPOWER);
		if (!ret){
			return false;
		}
	}

	// writeDebugStreamLine("Target:%d, Lift Position: %d", target, getLiftPosition());


	// writeDebugStreamLine("103");

	// CLAW LIFT UP
	ret = moveClawLiftUpAndStop(MAX_CLAWLIFT /*MID_CLAWLIFT + CLAWLIFT_OFFSET*/, powerClawLiftUp / 2);
	if (!ret){
		return false;
	}

	// writeDebugStreamLine("104");
	// waitUntil(getClawLiftPosition() >= MAX_CLAWLIFT);


	// while (getClawLiftPosition() < MAX_CLAWLIFT){
	// 	// moveClawLiftUp(power);
	// 	if (vexRT[Btn8L] == 1)
	// 	{
	// 		return false;
	// 	}
	// }

	// writeDebugStreamLine("105");


	// int tcur = getLiftPosition();
	// writeDebugStreamLine("LiftDownPickUpClawLiftUpLiftDown) CONE_COUNT: %d, target: %d, lift: %d", CONE_COUNT, target,  tcur);




	// PUSH CONE DOWN
	if (CONE_COUNT < 1){
		moveLiftDownAndStop_Touch(powerliftDown);
		// moveLiftDownAndStop(MIN_LIFT, powerliftDown);
		openClaw(powerClawOpen);
		wait1Msec(100);
		stopClaw();
	}
	else
	{
		// moveClawLiftUp(10);
		// writeDebugStreamLine("106");
		// if (CONE_COUNT >= 10)
		int tPower = powerliftDown;
		int liftPosition = getLiftPosition();
		if (liftPosition > SLOWDOWN_LIFT)
		{
			tPower = powerliftDown - 40;
			wait1Msec(400);
		}
		//ret = PushORUpCone(true);
		moveLiftDown(tPower);
		int waiter = 250;
		wait1Msec(waiter);
		stopLift();
		stopClawLift();

		if (!ret){
			return false;
		}

		// writeDebugStreamLine("107");

		stopClaw();

	}


	LIFT_STATUS = "";

	// writeDebugStreamLine("108");

	return true;
}

void ReleaseCone(){
	openClaw(powerClawOpen);
	wait1Msec(100);
	openClaw(powerClawClose);
	moveLiftUp(FULLPOWER / 2);
	wait1Msec(200);
	openClaw(powerClawOpen);
	moveLiftUp(FULLPOWER / 2);

}

void PushAndUpCone(){
	moveLiftDown(powerliftDown);
	wait1Msec(200);
	stopLift();

	// openClaw(powerClawOpen);
	// wait1Msec(200);

	// moveLiftUp(FULLPOWER);
	// openClaw(FULLPOWER);
	// wait1Msec(200);

	ReleaseCone();

	// wait1Msec(100);

	stopLift();
	stopClaw();
}

bool PushORUpCone(bool pushDown){

	bool ret = true;

	if (pushDown == true){
		moveLiftDown(powerliftDown);
		int waiter = 300;
		wait1Msec(waiter);
		stopLift();

	}
	else{
		// writeDebugStreamLine("Up");
		moveLiftUp(FULLPOWER);
		// writeDebugStreamLine("Up 1");
		int waiter = 200;

		// if (CONE_COUNT > 7){
		// 	waiter = 300;
		// }

		wait1Msec(waiter);
		// writeDebugStreamLine("Up 2");
		holdLift();
	}

	return ret;
}

void scoreStationaryGoal(){
	const int powerLift = FULLPOWER * 4 / 5;
	//const int powerLiftHalft = FULLPOWER / 2;
	//const int _powerClawLiftUp = powerClawLiftUp * 2 / 3;
	//const int _powerClawLiftDown = powerClawLiftDown * 2 / 3;
	int i = 0;

	while(true){
		// moveMobileLiftUp(20);
		wait1Msec(50);
		if (vexRT[Btn8D] == 1){

			stopLift();
			int clawLiftPosition = getClawLiftPosition();
			if (clawLiftPosition < MID_CLAWLIFT){
				moveClawLiftUpAndStop(MID_CLAWLIFT /*MID_CLAWLIFT + CLAWLIFT_OFFSET*/, powerClawLiftUp * 2 / 3);
				wait1Msec(100);
			}
			closeClaw(powerClawClose);
			moveLiftDown(FULLPOWER);
		}
		else if (vexRT[Btn8U] == 1){
			closeClaw(powerClawHold);
			moveLiftUp(FULLPOWER * 2 / 3);
			// PushORUpCone(false);
			// wait1Msec(100);
		}
		else if (vexRT[Btn7D] == 1){
			writeDebugStreamLine("7D> count: %d", i++);
			holdLift();
			closeClaw(powerClawHold);
			moveClawLiftDownAndStop(MIN_CLAWLIFT /*MID_CLAWLIFT - CLAWLIFT_OFFSET*/, powerClawLiftDown * 4 / 5 /*powerClawLiftDown*/);
			wait1Msec(100);
			while(vexRT[Btn7D] == 1)
			{
				moveLiftDown(powerLift);
			}
			holdLift();
			// while(true)
			// {
			// 	if (vexRT[Btn7D] == 1 || vexRT[Btn8L] == 1){
			// 		moveLiftDown(powerLift);
			// 		break;
			// 	}
			// 	else if (vexRT[Btn7U] == 1){
			// 		ReleaseCone();
			// 		break;
			// 	}
			// }

			// if (getLiftPosition() < 3600){
			// }
			// else
			// {
			// 	writeDebugStreamLine("Half");
			// 	moveLiftDown(powerLiftHalft);
			// }
		}
		else if (vexRT[Btn7U] == 1){
			ReleaseCone();

			if (CONE_COUNT > 1){
				CONE_COUNT--;
			}
			// CONE_COUNT_STATIONARY++;
		}
		else if (vexRT[Btn8L] == 1){
			stopLift();
			closeClaw(powerClawHold);
			break;
		}
	}
}

void moveLiftUpForStationaryGoal(int liftValue)
{
	//int offset = getLiftTargetPositionForStationaryGoal();
	//int target = liftValue + offset;

	bool ret = moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown);
	if (!ret){
		return;
	}

	closeClaw(FULLPOWER);

	ret = moveLiftDownAndHold_Touch(FULLPOWER);
	if (!ret){
		return;
	}

	// PICK UP CONE AND HOLD
	wait1Msec(200);

	// CONE HOLD
	closeClaw(powerClawHold);
	// CLAW LIFT UP

	// ret = moveLiftUpAndHold_Smart(target, FULLPOWER);
	// if (!ret){
	// 	stopLift();
	// 	return;
	// }

	// CLAW LIFT UP
	ret = moveClawLiftUpAndStop(MID_CLAWLIFT, powerClawLiftUp);
	if (!ret){
		stopClawLift();
		return;
	}

	bool moveUp = false;
	while (getStaionaryGoal() == 0){

		if (vexRT[Btn8L] == 1)
		{
			return;
		}
		else if (vexRT[Btn7U] == 1)
		{
			moveUp = true;
			moveLiftUp(FULLPOWER); //80
		}
		else if (vexRT[Btn7D] == 1)
		{
			moveUp = false;
			moveLiftDown(FULLPOWER); //80
		}

		if (vexRT[Btn7U] != 1 && vexRT[Btn7D] != 1)
		{
			if (moveUp){
				if (getLiftPosition() > 3000) {
					moveLiftUp(50);
				}
				else {
					holdLiftAuton();
				}
			}
			else
			{
				stopLift();
			}
		}
	}

	PushORUpCone(true);

	openClaw(powerClawOpen);
	wait1Msec(100);
	openClaw(powerClawClose);

	PushORUpCone(false);

	stopClaw();

	STATIONARY_GOAL_TARGET_NO_MG = getLiftPosition();
}

void moveLiftUpForStationaryGoalAuton(int liftValue)
{
	int offset = getLiftTargetPositionForStationaryGoal();
	int target = liftValue + offset;
	bool ret = true;

	// CONE HOLD
	closeClaw(30);

	// moveClawLiftUp(FULLPOWER);

	wait1Msec(100);

	// LIFT UP
	// ret = moveLiftUpAndHold_Smart(target, FULLPOWER);
	ret = moveLiftUpAndHold(target, FULLPOWER);
	if (!ret){
		return;
	}

	int tcur = getLiftPosition();
	writeDebugStreamLine("MoveLiftUpForStationaryGoal) CONE_COUNT: %d, target: %d, lift: %d", CONE_COUNT, target,  tcur);


	// CLAW LIFT UP
	// moveClawLiftUpAndStop(MAX_CLAWLIFT, FULLPOWER);

	while (getStaionaryGoal() == 0){

		if (vexRT[Btn8L] == 1)
		{
			return;
		}
	}
	MoveHelper(0);
	wait1Msec(200);

	PushORUpCone(true);

	openClaw(powerClawOpen);
	wait1Msec(100);
	openClaw(powerClawClose);

	PushORUpCone(false);

	stopClaw();
}

task clawLift()
{
	while (true)
	{
		wait1Msec(50);

		if (vexRT[Btn8R] == 1) {

			if (vexRT[Btn5U] == 1){
				moveClawLiftUpAndStop(MID_CLAWLIFT, /*50*/ powerClawLiftUp);
				// LIFT_STATUS = "manual";
				// moveClawLiftUp(powerClawLiftUp);
			}
			else if (vexRT[Btn5D] == 1){
				moveClawLiftDownAndStop(MID_CLAWLIFT, powerClawLiftDown);
				// LIFT_STATUS = "manual";
				// moveClawLiftDown(powerClawLiftDown);
			}
		}
		else if (vexRT[Btn8R] == 0) {

			if (vexRT[Btn5U] == 1){
				writeDebugStreamLine("5U - 1");
				pushMobileLiftUp();
				// closeClaw(FULLPOWER);
				holdLift();
				// stopLift();
				int clawLiftPosition = getClawLiftPosition();
				if (clawLiftPosition < MID_CLAWLIFT){
					int pClawLiftUp = powerClawLiftUp + 20;
					if (getLiftPosition() > 1400) {
						pClawLiftUp = powerClawLiftUp;
					}
					moveClawLiftUpAndStop(/*MAX_CLAWLIFT*/ MID_CLAWLIFT /* + CLAWLIFT_OFFSET*/, /*50*/ pClawLiftUp);
				}
				else{
					writeDebugStreamLine("5U - 2");
					pushMobileLiftUp();
					PushAndUpCone();
					CONE_COUNT++;

				}

			}
			else if (vexRT[Btn5D] == 1){
				// closeClaw(FULLPOWER);
				holdLift();
				// stopLift();
				int clawLiftPosition = getClawLiftPosition();
				if (clawLiftPosition < MID_CLAWLIFT){
					moveClawLiftUpAndStop(/*MAX_CLAWLIFT */ MID_CLAWLIFT /* + CLAWLIFT_OFFSET*/, 10 /*powerClawLiftUp*/);
				}
				else{
					writeDebugStreamLine("5d");
					pushMobileLiftUp();

					openClaw(powerClawOpen);

					while(true){

						if (vexRT[Btn8L] == 1)
						{
							// stopClaw();
							break;
						}
						else if (vexRT[Btn8D] == 1)
						{
							break;
						}
					}

					// stopClaw();

					CONE_COUNT++;

				}
			}
		}

		if (LIFT_STATUS == "manual" && vexRT[Btn5U] == 0 && vexRT[Btn5D] == 0){
			stopClawLift();
		}
	}
}

task claw()
{
	while (true)
	{
		wait1Msec(50);

		if (vexRT[Btn8R] == 0) {
			if (vexRT[Btn6U] == 1){

				closeClaw(FULLPOWER);

				// int liftPos = getLiftPosition();
				// int clawLiftPos = getClawLiftPosition();

				// if (liftPos < MIN_LIFT && clawLiftPos < MIN_CLAWLIFT){
				// 	closeClaw(FULLPOWER);
				// 	bool ret = moveLiftDownAndHold_Touch(FULLPOWER);
				// 	if (!ret){
				// 		moveLiftUpAndStop(liftPos + 20, 30);
				// 		// return;
				// 	}
				// }
			}
			else if (vexRT[Btn6D] == 1){
				openClaw(FULLPOWER);
				wait1Msec(250);
				stopClaw();
			}
		}
	}
}


task usercontrol()
{
	startTask(move);
	startTask(lift);
	startTask(claw);
	startTask(clawLift);
	// startTask(updateSensors);
	// User control code here, inside the loop
	//int on = 0;
	while (true)
	{
		wait1Msec(50);


		int btnLCD = nLCDButtons;
		if (btnLCD > 0)
		{
			AutonomousSelector(btnLCD);
			// startTask(displayBatteryLevelOnLCD);
		}

		while(vexRT[Btn8L] == 1){
			stopAll();
		}
	}
}

// -------------------------------------
// SENSORS
int getLiftPosition()
{
	return SensorValue(liftPosition);
}

int getLiftDown()
{
	return SensorValue(liftDown);
}

int getClawLiftPosition()
{
	return SensorValue(clawLiftPosition);
}

int getMobileLiftPosition()
{
	return SensorValue(mobileLiftPosition);
	// if (GLOGAL_MOBILELIFTPOSITION > 0) {
	// 	writeDebugStreamLine("GLOGAL_MOBILELIFTPOSITION: %d", GLOGAL_MOBILELIFTPOSITION);
	// 	return GLOGAL_MOBILELIFTPOSITION;
	// }
	// else
	// {
	// int value = SensorValue(mobileLiftPosition);
	// writeDebugStreamLine("SensorValue: %d", value);
	// return SensorValue(value);
	// }
}

int getMobileLiftDown()
{
	return SensorValue(mobileLiftDown);
}

int getStaionaryGoal()
{
	return SensorValue(stationaryGoal);
}

void ClearEncoder() {
	// writeDebugStreamLine("ClearEncoder");
	nMotorEncoder[wheelRight] = 0;
	// SensorValue[Wheel] = 0;
}

int getLine(LineType lineType)
{
	switch (lineType){
	case Right:
		return getLineRight();
	case Left:
		return getLineLeft();
	case Back:
		return getLineBack();
	default:
		return getLineBack();
	}
}

int getLineDiff(LineType lineType)
{
	int tline = getLine(lineType);
	int diff = 0;
	switch (lineType){
	case Right:
		diff = COLOR_BASE_RIGHT - tline;
		break;
	case Left:
		diff = COLOR_BASE_LEFT - tline;
		break;
	case Back:
		diff = COLOR_BASE_BACK - tline;
		break;
	default:
		diff =  COLOR_BASE_BACK - tline;
		break;
	}

	if (diff > 200){
		diff = 10;
	}
	return diff;
}

int getLineBack()
{
	return SensorValue(lineBack);
}

int getLineRight()
{
	return SensorValue(lineRight);
}

int getLineLeft()
{
	return SensorValue(lineLeft);
}



// END SENSORS


// -------------------------------------
// STOPS
void stopAll(){

	LIFT_STATUS = "";

	motor[armLiftLeftB] = 0;
	// motor[armLiftLeftT] = 0;
	motor[armLiftRightB] = 0;
	// motor[armLiftRightT] = 0;
	motor[clawMotor] = 0;
	motor[clawLiftL] = 0;
	motor[clawLiftR] = 0;
	motor[mobileLift] = 0;
	motor[wheelLeft] = motor[wheelLeftMid] = 0;
	motor[wheelRight] = motor[wheelRightMid] = 0;
}
// END STOPS

// -------------------------------------
// LIFT
int getLiftTargetPositionForStationaryGoal(){
	if (STATIONARY_GOAL_TARGET_NO_MG < 0){
		// int offset = 30;
		// if (CONE_COUNT_STATIONARY > 7){
		// 	offset = 50;
		// }
		// return CONE_COUNT_STATIONARY * offset;
		return 100;
	}
	else
	{
		return STATIONARY_GOAL_TARGET_NO_MG;
	}
}

int getLiftTargetPosition(){
	int offset = 100; //120
	int count = CONE_COUNT - 2;

	if (CONE_COUNT >= 11){
		count = CONE_COUNT + 3;
	}
	// else if (1 <= CONE_COUNT && CONE_COUNT < 3){
	else if (1 == CONE_COUNT || 2 == CONE_COUNT){
		return CONE_COUNT * (offset / 2);
	}
	return count * offset;
}


void holdLift(){
	LIFT_STATUS = "holdLift";
	if (getLiftPosition() > 3400) {
		moveLift(20);
	}
	else {
		moveLift(0);
	}
}

void holdLiftAuton() {
	LIFT_STATUS = "holdLift";
	moveLift(25);
}

void holdDownLift(){
	LIFT_STATUS = "holdLift";

	// writeDebugStreamLine("holdDownLift");
	moveLift(-15);
}

void stopLift(){
	LIFT_STATUS = "stopLift";

	moveLift(0);
}

void moveLiftUp(int power) {

	// LIFT_STATUS = "moveLiftUp";

	if (getLiftPosition() >= MAX_LIFT)
	{
		return;
	}
	moveLift(power);
}

void moveLiftDown(int power) {

	// LIFT_STATUS = "moveLiftDown";

	// if (getLiftPosition() <= MIN_LIFT)
	if (getLiftDown() == 1)
	{
		return;
	}
	moveLift(-power);
}

void moveLift(int power){

	// LIFT_STATUS = "moveLift";

	int newPower = power;
	// motor[armLiftLeftT] = -newPower; // Left Top. Comment this for 2 motors lift design.
	motor[armLiftLeftB] = newPower;
	motor[armLiftRightB] = newPower;
	// motor[armLiftRightT] = -newPower; // Right Top. Comment this for 2 motors lift design.
}

// HELPER
bool moveLiftUpAndStop(int distance, int power) {

	int current = getLiftPosition();

	while (current < distance){

		wait1Msec(50);

		//int previous = current;

		moveLiftUp(power);
		if (vexRT[Btn8L] == 1)
		{
			stopLift();
			return false;
		}

		current = getLiftPosition();
	}
	stopLift();

	return true;
}

bool moveLiftUpAndHold_Smart(int distance, int power) {

	if (distance > MAX_LIFT){
		distance = MAX_LIFT;
	}


	float newDis = distance * 4 / 5;

	// if (CONE_COUNT > 7){
	// 	 newDis = distance * 4 / 5;
	// }

	int current = getLiftPosition();

	while (current < distance){

		wait1Msec(50);

		moveLiftUp(power);
		if (vexRT[Btn8L] == 1)
		{
			stopLift();
			return false;
		}

		current = getLiftPosition();

		if (current >= newDis){
			moveClawLiftUp(30);
		}
	}
	holdLift();

	return true;
}

bool moveLiftUpAndHold(int distance, int power) {

	if (distance > MAX_LIFT){
		distance = MAX_LIFT;
	}

	// float offset = 0;
	int current = getLiftPosition();

	while (current /*+ offset*/ < distance){

		wait1Msec(50);

		//int previous = current;


		moveLiftUp(power);
		if (vexRT[Btn8L] == 1)
		{
			stopLift();
			return false;
		}

		current = getLiftPosition();

		// offset = current - previous;
		// offset = offset * 5;

	}
	holdLift();

	return true;
}

bool moveLiftUpAndHoldAuton(int distance, int power) {

	// float offset = 0;
	int current = getLiftPosition();

	while (current /*+offset*/ < distance){

		wait1Msec(50);

		//int previous = current;


		moveLiftUp(power);
		if (vexRT[Btn8L] == 1)
		{
			stopLift();
			return false;
		}

		current = getLiftPosition();

		// offset = current - previous;
		// offset = offset * 5;

	}
	holdLiftAuton();

	return true;
}

bool moveLiftDownAndHold_Touch(int power) {

	clearTimer(T2);
	while (getLiftDown() == 0){
		// wait1Msec(50);

		moveLift(-power);
		if (vexRT[Btn8L] == 1 || time1[T2] > 2000)
		{
			stopLift();
			return false;
		}
	}

	holdDownLift();

	return true;
}

bool moveLiftDownAndStop_Touch(int power) {

	clearTimer(T2);
	while (getLiftDown() == 0){
		// wait1Msec(50);

		moveLift(-power);
		if (vexRT[Btn8L] == 1 || time1[T2] > 1000)
		{
			stopLift();
			return false;
		}
	}

	stopLift();

	return true;
}


bool moveLiftDownAndStop(int distance, int power) {

	// clearTimer(T2)
	int cur = getLiftPosition();
	int prev = -100;
	int count = 0;
	while (cur > distance){

		wait1Msec(50);

		moveLiftDown(power);

		if ( /*time1[T2] >= 5000) ||*/ vexRT[Btn8L] == 1 || vexRT[Btn7U] == 1 || vexRT[Btn7D] == 1) {
			stopLift();
			return false;
		}

		if ((cur/10) == (prev/10)) {
			count++;

			if (count > 3)
			{
				writeDebugStreamLine("moveLiftDownAndStop) count: %d", count);
				stopLift();
				return false;
			}
		}

		else {
			count = 0;
		}

		prev = cur;
		cur = getLiftPosition();
	}
	// wait1Msec(200);
	stopLift();

	return true;
}

bool moveLiftDownAndHold(int distance, int power) {

	// clearTimer(T2)
	int cur = getLiftPosition();
	int prev = -100;
	int count = 0;
	while (cur > distance){

		wait1Msec(50);

		moveLiftDown(power);

		if ( /*time1[T2] >= 5000) ||*/ vexRT[Btn8L] == 1) {
			break;
		}

		if ((cur/10) == (prev/10)) {
			count++;

			if (count > 3)
			{
				writeDebugStreamLine("moveLiftDownAndStop) count: %d", count);
				stopLift();
				return false;
			}
		}

		else {
			count = 0;
		}

		prev = cur;
		cur = getLiftPosition();
	}
	// wait1Msec(200);
	holdDownLift();

	return true;
}
// END MOVES

// -------------------------------------
// CLAW LIFT
void stopClawLift(){
	moveClawLift(0);
}

void moveClawLiftUp(int power){
	moveClawLift(power);
}

void moveClawLiftDown(int power){
	moveClawLift(-power);
}

void moveClawLift(int power){
	int newPower = power;

	motor[clawLiftL] = newPower;
	motor[clawLiftR] = -newPower;
}

bool moveClawLiftUpAndStop(int distance, int power) {

	while (getClawLiftPosition() < distance){
		moveClawLiftUp(power);
		if (vexRT[Btn8L] == 1)
		{
			return false;
		}
	}
	stopClawLift();

	return true;
}

bool moveClawLiftDownAndHold(int distance, int power) {
	bool ret = moveClawLiftDownAndHelper(distance, power);
	moveClawLiftDown(10);

	if (!ret)
	{
		stopClawLift();
	}
	return ret;
}


bool moveClawLiftDownAndStop(int distance, int power) {
	bool ret = moveClawLiftDownAndHelper(distance, power);
	stopClawLift();
	return ret;
}

bool moveClawLiftDownAndHelper(int distance, int power) {

	clearTimer(T4);
	int clawLiftPosition = getClawLiftPosition();

	// writeDebugStreamLine("In <<<<<<<<<<<<<<<<<<<<<<<<");
	while (clawLiftPosition > distance || clawLiftPosition < ERROR_CLAWLIFT){
		wait1Msec(50);

		clawLiftPosition = getClawLiftPosition();
		moveClawLiftDown(power);

		// writeDebugStreamLine("clawLiftPosition: %d", clawLiftPosition);

		// if (abs(temp - clawLiftPosition) > 320){
		// 	writeDebugStreamLine("invalid");
		// 	mobileLiftPosition = MAX_MOBILELIFT;
		// }

		if (vexRT[Btn8L] == 1)
		{
			return false;
		}

		if (time1[T4] >= 2000) {
			return false;
		}

	}
	// stopClawLift();
	// writeDebugStreamLine("Out");


	return true;
}
// END CLAW LIFT


// -------------------------------------
// CLAW
void stopClaw(){
	// writeDebugStreamLine("stopClaw");
	moveClaw(0);
}

void openClaw(int power){
	moveClaw(power);
}

void closeClaw(int power){
	moveClaw(-power);
}

void moveClaw(int power){
	motor[clawMotor] = power;
}

// -------------------------------------
// MOBILE LIFT
void stopMobileLift(){
	moveMobileLift(0);
}

void moveMobileLiftUp(int power){
	moveMobileLift(power);
}

void pushMobileLiftUp(){
	moveMobileLift(0);
}

void moveMobileLiftDown(int power){
	moveMobileLift(-power);
}


void moveMobileLift(int power){
	motor[mobileLift] = power;
}

void moveMobileLiftUpToStackAuton(int powerLift) {
	moveLiftUpAndStop(LIFT_PICK_MOBILE, powerLift);
	wait1Msec(200);
	moveMobileLiftUpAndStop();
}

void moveMobileLiftUpAndStop() {

	while (getMobileLiftPosition() < MAX_MOBILELIFT){
		moveMobileLiftUp(FULLPOWER);
		if (vexRT[Btn8L] == 1)
		{
			break;
		}
	}

	stopMobileLift();

	CONE_COUNT = 0;
	// STATIONARY_GOAL_TARGET = 0;
}

void moveMobileLiftUpAndStopDrivingSkill() {

	while (getMobileLiftPosition() < DRIVING_MOBILELIFT){
		moveMobileLiftUp(FULLPOWER);
		if (vexRT[Btn8L] == 1)
		{
			break;
		}
	}

	stopMobileLift();

	CONE_COUNT = 0;
	// STATIONARY_GOAL_TARGET = 0;
}

//void moveMobileLiftUpAuton(){
//	while (getMobileLiftDown() == 1) {
//		moveMobileLiftUp(FULLPOWER);
//	}
//}

void moveMobileLiftDownToPickUp() {
	int liftPosition = getLiftPosition();
	bool higher = true;

	if (CONE_COUNT == 0){
		higher = false;
		bool ret = moveLiftUpAndStop(LIFT_PICK_MOBILE, FULLPOWER);
		if (!ret){
			return;
		}
	}
	else if (liftPosition <= LIFT_PICK_MOBILE)
	{
		higher = false;
		openClaw(powerClawOpen);
		wait1Msec(50);

		bool ret = moveLiftUpAndStop(LIFT_PICK_MOBILE, FULLPOWER);
		if (!ret){
			return;
		}

		stopClaw();

		// wait1Msec(200);
		// moveClawLiftUpAndStop(FULLPOWER);
		wait1Msec(300);

	}
	else if (liftPosition > LIFT_PICK_MOBILE)
	{
		moveLiftDown(FULLPOWER / 2);
		wait1Msec(100);
		stopLift();
	}

	moveMobileLiftDownAndStop(higher);
}

void moveMobileLiftDownToPickUpAuton() {
	// if (CONE_COUN < 4){
	// 	closeClaw(10);
	// }

	bool ret = moveLiftUpAndHold(LIFT_PICK_MOBILE, FULLPOWER);
	if (!ret){
		return;
	}

	holdLift();

	moveClawLiftUpAndStop(MID_CLAWLIFT, powerClawLiftUp);
	// wait1Msec(200);
	moveMobileLiftDownAndStopAuton(0);
}


void SetInitialPositionAuton() {
	closeClaw(40);

	bool ret = moveLiftUpAndHold(LIFT_PICK_MOBILE - 50, FULLPOWER);
	if (!ret){
		return;
	}

	holdLift();

	moveMobileLiftDown(FULLPOWER);
	// wait1Msec(200);
	moveMobileLiftDownAndStopAuton(0);
}


void moveMobileLiftDownAndStop(bool higher) {
	int mobileLiftDown = getMobileLiftDown();
	int mobileLiftPosition = getMobileLiftPosition();
	//int previousMLPosition = mobileLiftPosition;

	// writeDebugStreamLine("1 moveMobileLiftDownAndStop> mobileLiftPosition: %d, STOP_MOBILELIFT: %d", mobileLiftPosition, STOP_MOBILELIFT);


	// openClaw(FULLPOWER/2);
	// wait1Msec(100);
	// stopClaw();

	while (mobileLiftDown == 0){
		// writeDebugStreamLine("in");
		wait1Msec(50);

		moveMobileLiftDown(FULLPOWER);
		moveClawLiftDown(30);

		// 		int liftPos = getLiftPosition();
		// +		if (CONE_COUNT > 0) {
		// +			writeDebugStreamLine("clawLift");
		// +			moveClawLiftDown(30 + 20);
		// +		}


		if (vexRT[Btn8L] == 1)
		{
			break;
		}

		int temp = mobileLiftPosition;
		mobileLiftDown = getMobileLiftDown();
		mobileLiftPosition = getMobileLiftPosition();
		if (abs(temp - mobileLiftPosition) > 300){
			// writeDebugStreamLine("invalid");
			mobileLiftPosition = MAX_MOBILELIFT;
		}

		// writeDebugStreamLine("2 moveMobileLiftDownAndStop> mobileLiftPosition: %d, STOP_MOBILELIFT: %d", mobileLiftPosition, STOP_MOBILELIFT);


		if (mobileLiftDown == 1 || mobileLiftDown != 0)
		{
			// writeDebugStreamLine("mobileLiftDown: %d", mobileLiftDown);
			break;
		}


		if (mobileLiftPosition < STOP_MOBILELIFT){
			// writeDebugStreamLine("3 moveMobileLiftDownAndStop> mobileLiftPosition: %d, STOP_MOBILELIFT: %d", mobileLiftPosition, STOP_MOBILELIFT);

			moveClawLiftDown(20);

			if (higher){
				stopMobileLift();
				// moveClawLiftDown(40);
				// int tcur = getLiftPosition();
				// moveLiftUpAndStop(tcur + 70, 100);
			}

		}
	}

	// writeDebugStreamLine("out");
	stopMobileLift();
	stopClawLift();

	//int liftPosition = getLiftPosition();
	if (higher) {
		// if (liftPosition > LIFT_PICK_MOBILE){
		wait1Msec(200);
		openClaw(FULLPOWER);
		wait1Msec(200);
		int tcur = getLiftPosition();
		moveLiftUpAndStop(tcur + 100, 100);
		moveClawLiftUpAndStop(MAX_CLAWLIFT, FULLPOWER / 2);
		// int tcur = getLiftPosition();
		// moveLiftUpAndStop(tcur + 100, 100);
	}

	stopClaw();
}

void moveMobileLiftDownAndStopAuton(int distance) {
	bool ret = moveLiftUpAndHoldAuton(LIFT_PICK_MOBILE, FULLPOWER);
	if (!ret){
		return;
	}

	if (distance == 0) {
		while (getMobileLiftDown() == 0){
			wait1Msec(50);
			moveMobileLiftDown(FULLPOWER);
			if (vexRT[Btn8L] == 1)
			{
				break;
			}
		}
		stopMobileLift();
		// moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown / 2);
	}
	else {
		// writeDebugStreamLine("moveMobileLiftDownAndStopAuton: MGLiftPosistion %d, distance: %d", getMobileLiftPosition(), distance);

		moveMobileLiftDown(FULLPOWER);
		wait1Msec(300);

		int moveLiftPosition = getMobileLiftPosition();
		while (moveLiftPosition > distance){
			moveLiftPosition = getMobileLiftPosition();
			// writeDebugStreamLine("IN: moveLiftPosition: %d", moveLiftPosition);

			wait1Msec(50);
			// writeDebugStreamLine("IN 2");
			// writeDebugStreamLine("IN 3");
			if (vexRT[Btn8L] == 1)
			{
				// writeDebugStreamLine("IN 4");
				break;
			}
		}
		// writeDebugStreamLine("OUT");
		stopMobileLift();
	}
}
// END MOBILE LIFT

// -------------------------------------
// MOVE
bool MoveLine(LineType lineType, int power) {
	// ClearEncoder();

	//writeDebugStreamLine("MoveValue_Accel) Started ===");
	//int velocity = 0;
	//int position = 0;
	//int value = 0;
	clearTimer(T3);
	int newPower = AdjustPowerUsingExternalBatteryLevelToMove(power);

	int diff = getLineDiff(lineType);
	MoveHelper(newPower);
	// MoveHelper(newPower);

	while (true){
		// writeDebugStreamLine("in");
		// wait1Msec(20);

		if (time1[T3] >= 2000){
			writeDebugStreamLine("timeout");
			MoveHelper(0);
			return false;
		}

		diff = getLineDiff(lineType);
		// writeDebugStreamLine("diff) diff: %d", diff);

		if (diff > COLOR_DIFF){
			// writeDebugStreamLine("MoveLine) diff: %d", diff);

			// writeDebugStreamLine("diff!!!");
			break;
		}
		// wait1Msec(100);
	}
	// writeDebugStreamLine("out");

	return true;
}

bool MoveLineStop(LineType lineType, int power) {
	bool ret = MoveLine(lineType, power);
	MoveHelper(0);
	return ret;
}

//void MoveForward(int distance) {
//	MoveIEM(distance, 80);
//}

//void MoveBackword(int distance) {
//	MoveIEM(distance, -80);
//}

void MoveIEM(int distance, int power) {
	ClearEncoder();
	clearTimer(T3);
	int position = nMotorEncoder[wheelRight];
	power = AdjustPowerUsingExternalBatteryLevelToMove(power);
	while (abs(position)<abs(distance)){
		position = nMotorEncoder[wheelRight];
		// writeDebugStreamLine("MoveValue_IEM) pos: %d, distance: %d ", position, distance);

		if (time1[T3] >= 2000)
		{
			break;
		}
		MoveHelper(power);
	}
	MoveHelper(0);
}

void MoveHelper(int power)
{
	motor[wheelLeft] = motor[wheelLeftMid] = -power;
	motor[wheelRight] = motor[wheelRightMid] = power;
}

bool TurnLine(LineType lineType, int power) {
	// wait1Msec(time);
	clearTimer(T3);
	int newPower = AdjustPowerUsingExternalBatteryLevelToMove(power);

	int diff = getLineDiff(lineType);

	// writeDebugStreamLine("TurnLine - before In) diff: %d", diff);
	if (diff > COLOR_DIFF){
		// writeDebugStreamLine("diff!!!");
		return true;
	}

	TurnHelper(newPower);
	// TurnHelperRight(newPower);
	// MoveHelper(newPower);

	while (true){
		// writeDebugStreamLine("in");
		// wait1Msec(50);

		if (time1[T3] >= 2000){

			TurnHelper(0);
			writeDebugStreamLine("timeout");
			return false;
		}

		diff = getLineDiff(lineType);


		// if (diff > 20){
		// writeDebugStreamLine("TurnLine) diff: %d", diff);
		// 	// writeDebugStreamLine("diff!!!");
		// 	// break;
		// }

		if (diff > COLOR_DIFF){
			// writeDebugStreamLine("TurnLine) diff: %d", diff);
			// writeDebugStreamLine("diff!!!");
			break;
		}
		// wait1Msec(100);
	}
	// writeDebugStreamLine("out");

	TurnHelper(0);

	return true;
}

void TurnGyroRight(float angle) {
	const float valuePerangle = 8.7;
	float distance = valuePerangle * angle;
	TurnGyro(distance, -45);
}

void TurnGyroLeft(float angle) {
	const float valuePerangle = 8.9;
	float distance = valuePerangle * angle;
	TurnGyro(distance, 45);
}

void TurnGyro(int distance, int power) {
	// wait1Msec(time);
	SensorValue[GyroPosition] = 0;
	int newPower = power;
	while (abs(SensorValue[GyroPosition])<abs(distance)){
		TurnHelper(newPower);
	}
	TurnHelper(0);
}

void TurnHelper(int power)
{
	motor[wheelLeft] = motor[wheelLeftMid] = -power;
	motor[wheelRight] = motor[wheelRightMid] = -power;
}

// -------------------------------------
// ADJUST BATTERY
int AdjustPowerUsingExternalBatteryLevelToMove(int originalPower)
{
	// return originalPower;
	float batteryLevel = SensorValue[ExternalBatteryValue] * 3.57;

	float batteryOffset = 7500 / batteryLevel;
	int adjustedPower = originalPower * batteryOffset;
	//  writeDebugStreamLine("(AjustBattery) BatterLevel: %d - OriginalPower: %d - WantedPower: %d", AdjustBatteryLevel, OriginalPower, WantedPower);
	return adjustedPower;
}


// END ADJUST BATTERY


// -------------------------------------
// LCD
void AutonomousSelector(int btnLCD)
{
	writeDebugStreamLine("AutonomousSelector) button: %d", btnLCD);

	bLCDBacklight=true;

	displayBatteryLevelOnLCD(autonomousMode);
	int autonomousModeValue = autonomousMode;

	int button = 0;
	while(true)
	{
		button = nLCDButtons;

		if (button == 2 || vexRT[Btn8L] == 1)
		{
			displayBatteryLevelOnLCD(autonomousModeValue);

			bLCDBacklight=false;
			break;
		}
		else if (button == 1 || vexRT[Btn6U] == 1)
		{
			if (autonomousModeValue > 1)
			{
				autonomousModeValue--;
			}

			DisplayAutonomousMode(autonomousModeValue);
		}
		else if (button == 4 || vexRT[Btn6D] == 1)
		{
			if (autonomousModeValue < 8)
			{
				autonomousModeValue++;
			}

			DisplayAutonomousMode(autonomousModeValue);
		}
		// 	+	else{
		// +
		// +			string text;
		// +			sprintf(text, "Selected: %d", nMotorEncoder[wheelRight]); //Build the value to be displayed
		// +
		// +			//Display the Primary Robot battery voltage
		// +			// displayLCDString(0, 0, "Autonomous: ");
		// +			clearLCDLine(1);
		// +			displayLCDString(0, 0, text);
		// +
		// +
		// +		}
	}

	bLCDBacklight=false;
}

void DisplayAutonomousMode(int autonomousModeValue)
{
	writeDebugStreamLine("DisplayAutonomousMode) autonomousModeValue: %d", autonomousModeValue);

	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

	string text;
	//}
	switch(autonomousModeValue)
	{
	case 1:
		{
			text = "20_L";
			break;
		}
	case 2:
		{
			text = "20_R";
			break;
		}
	case 3:
		{
			text = "autonCone5_Left";
			break;
		}
	case 4:
		{
			text = "autonCone5_Right";
			break;
		}
	case 5:
		{
			text = "Station_L";
			break;
		}
	case 6:
		{
			text = "Station_R";
			break;
		}
	case 7:
		{
			text = "DEF_L";
			break;
		}
	case 8:
		{
			text = "DEF_R";
			break;
		}
	case 9:
		{
			text = "ProgrammingSkill";
			break;
		}
	case 10:
		{
			text = "drivingSkill";
			break;
		}
	case 11:
		{
			text = "None";
			break;
		}
	}

	//Display the Primary Robot battery voltage
	// displayLCDString(0, 0, "Autonomous: ");
	displayLCDString(0, 0, text);

	wait1Msec(500);
}

void displayBatteryLevelOnLCD(int autonomousModeValue)
{
	writeDebugStreamLine("displayBatteryLevelOnLCD) autonomousModeValue: %d", autonomousModeValue);

	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

	autonomousMode = autonomousModeValue;

	string text;
	sprintf(text, "Selected: %d", autonomousMode); //Build the value to be displayed

	//Display the Primary Robot battery voltage
	// displayLCDString(0, 0, "Autonomous: ");
	displayLCDString(0, 0, text);

	wait1Msec(800);

	//while(true)                                                        // An infinite loop to keep the program running until you terminate it
	//{
	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

	string mainBattery, externalBattery;

	//Display the Primary Robot battery voltage
	displayLCDString(0, 0, "Primary: ");
	sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
	displayNextLCDString(mainBattery);

	//float externalBatteryLevel = SensorValue[ExternalBatteryValue];
	//Display the Backup battery voltagef
	displayLCDString(1, 0, "External: ");
	sprintf(externalBattery, "%1.2f%c", (SensorValue[ExternalBatteryValue] * 3.57)/1000.0, 'V');    //Build the value to be displayed
	displayNextLCDString(externalBattery);

	//Short delay for the LCD refresh rate
	wait1Msec(500);
}

void programmingSkill() {
	int powerToMove = 70;
	//int turnPower = 125;
	int powerToTurn = 65;

	// 1st

	closeClaw(20);

	moveMobileLiftDownToPickUpAuton();
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	moveMobileLiftUpAndStop(); // Pick up FIRST MG
	wait1Msec(GLOBAL_WAITER_LONG);

	moveLiftDownAndHold(MIN_LIFT, powerliftDown);
	wait1Msec(GLOBAL_WAITER);

	openClaw(powerClawOpen); //for one cone only!
	wait1Msec(400); //for one cone only!
	stopClaw(); //for one cone only!
	wait1Msec(GLOBAL_WAITER);

	MoveHelper(-powerToMove); //for one cone only!
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(0);

	TurnGyroRight(200);
	wait1Msec(GLOBAL_WAITER + 100);

	MoveIEM(80, powerToMove);
	// TurnLine(Back, powerToTurn - 22); // Right turn
	wait1Msec(GLOBAL_WAITER);
	// TurnLine(Back, -(powerToTurn - 35)); // Right turn
	// wait1Msec(GLOBAL_WAITER + 300);

	// MoveLineStop(Left, FULLPOWER);
	// wait1Msec(500);
	// TurnGyro(90, -turnPower); // 125, 50
	TurnGyroLeft(9);
	wait1Msec(200);
	MoveLineStop(Back, FULLPOWER);
	// wait1Msec(GLOBAL_WAITER);
	MoveIEM(400, 100);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(50);
	wait1Msec(400);
	MoveHelper(0);
	moveMobileLiftDownToPickUp(); // Deploy First MG
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(250 + 100, -100);
	wait1Msec(GLOBAL_WAITER);
	moveMobileLiftUpAndStop();
	wait1Msec(GLOBAL_WAITER_LONG);
	moveLiftDownAndStop(LIFT_PICK_MOBILE, powerliftDown); // LIFT DOWN
	wait1Msec(GLOBAL_WAITER);

	MoveLineStop(Left, powerToMove - 45); //move a bit forward
	wait1Msec(GLOBAL_WAITER + 200);
	MoveLineStop(Left, -(powerToMove - 45)); //move a bit backward
	wait1Msec(GLOBAL_WAITER + 200);
	TurnLine(Back, -(powerToTurn - 20));  //turn left
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(90, -80); //move back
	wait1Msec(GLOBAL_WAITER + 200);

	TurnGyroLeft(60);
	// TurnLine(Left, -(powerToTurn - 30 - 5));  //align with bar
	HitBarAndAlign();

	// 2nd
	moveMobileLiftDownToPickUpAuton();

	MoveHelper(-(powerToMove - 25));
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove - 25);
	wait1Msec(GLOBAL_WAITER + 800);

	MoveLineStop(Right, powerToMove - 20);

	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove); //move forward to grab SECOND MG
	wait1Msec(GLOBAL_WAITER + 600);
	MoveHelper(0); //move forward to grab MG

	moveMobileLiftUpAndStop(); 			//pick up SECOND MG
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveLineStop(Right, FULLPOWER);
	wait1Msec(500);

	TurnGyroRight(14);

	moveLiftDownAndStop(LIFT_PICK_MOBILE, powerliftDown);
	// TurnGyro(80, turnPower); // 125, 50

	wait1Msec(200);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(500, 110);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(50);
	wait1Msec(600);
	MoveHelper(0);
	moveMobileLiftDownToPickUp(); // Deploy SECOND MG
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(250 + 100, -100);
	wait1Msec(GLOBAL_WAITER);
	moveMobileLiftUpAndStop();
	wait1Msec(GLOBAL_WAITER_LONG);
	moveLiftDownAndStop(LIFT_PICK_MOBILE, powerliftDown); // LIFT DOWN
	wait1Msec(GLOBAL_WAITER);

	MoveLineStop(Left, powerToMove - 45); //move a bit forward
	wait1Msec(GLOBAL_WAITER + 200);
	MoveLineStop(Left, -(powerToMove - 45)); //move a bit backward
	wait1Msec(GLOBAL_WAITER + 200);
	TurnLine(Back, -(powerToTurn - 20));  //turn left
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(90, -80); //move back
	wait1Msec(GLOBAL_WAITER + 200);
	// TurnLine(Left, -(powerToTurn - 30));  //align with bar

	TurnGyroLeft(60);

	HitBarAndAlign();

	// 3rd
	moveMobileLiftDownToPickUpAuton();
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	moveMobileLiftUpAndStop(); // Pick up 3rd MG


	MoveHelper(-powerToMove);
	wait1Msec(300);
	MoveLineStop(Back, -FULLPOWER);

	wait1Msec(GLOBAL_WAITER + 200);
	TurnGyroRight(200);



	// wait1Msec(GLOBAL_WAITER_LONG);

	// moveLiftDownAndHold(MIN_LIFT, powerliftDown);
	// wait1Msec(GLOBAL_WAITER);

	// openClaw(powerClawOpen); //for one cone only!
	// wait1Msec(400); //for one cone only!
	// stopClaw(); //for one cone only!
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER + 200);
	// TurnGyroRight(200);
	// TurnLine(Back, -(powerToTurn));
	// wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Left, -(powerToTurn - 22));
	// wait1Msec(GLOBAL_WAITER_LONG);
	MoveHelper(50);
	wait1Msec(GLOBAL_WAITER + 100);
	moveMobileLiftDownToPickUpAuton(); // deploy 3rd MG
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveIEM(150, -80);
	wait1Msec(GLOBAL_WAITER);

	TurnGyroRight(200);
	wait1Msec(GLOBAL_WAITER_LONG);
	HitBarAndAlign();

	// 4th
	MoveHelper(-(powerToMove - 25));
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove - 25);
	wait1Msec(GLOBAL_WAITER + 800);

	MoveLineStop(Right, powerToMove - 20);

	wait1Msec(GLOBAL_WAITER_LONG);

	// wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove); //move forward to grab 4th MG
	wait1Msec(GLOBAL_WAITER + 600);
	MoveHelper(0); //move forward to grab MG

	moveMobileLiftUpAndStop(); 			//pick up 4th MG
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove);
	wait1Msec(500);
	// MoveLineStop(Right, FULLPOWER);
	// wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER);

	moveMobileLiftDownToPickUpAuton(); // Deploy SECOND MG
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);


}

void programmingSkill_1() {
	int powerToMove = 70;
	int turnPower = 125;
	//int powerToTurn = 65;
	moveMobileLiftDownToPickUpAuton();
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	moveMobileLiftUpAndStop(); // Pick up FIRST MG
	wait1Msec(GLOBAL_WAITER_LONG);

	moveLiftDownAndHold(MIN_LIFT, powerliftDown);
	wait1Msec(GLOBAL_WAITER);

	openClaw(powerClawOpen); //for one cone only!
	wait1Msec(400); //for one cone only!
	stopClaw(); //for one cone only!
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, -FULLPOWER);
	wait1Msec(GLOBAL_WAITER + 200);
	TurnGyroRight(200);
	// TurnLine(Back, -(powerToTurn));
	// wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Left, -(powerToTurn - 22));
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveHelper(30);
	wait1Msec(GLOBAL_WAITER + 100);
	moveMobileLiftDownToPickUpAuton(); // deploy FIRST MG
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveIEM(150, -80);
	wait1Msec(GLOBAL_WAITER);

	TurnGyroRight(200);
	// TurnLine(Back, (powerToTurn - 22)); //turn right 180"
	// wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Left, (powerToTurn - 25));  //align with SECOND MG
	wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Left, -(powerToTurn - 32));  //align with SECOND MG
	// wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Right, -(powerToTurn - 22));  //align with SECOND MG
	// wait1Msec(GLOBAL_WAITER_LONG);
	HitBarAndAlign();
	MoveHelper(-(powerToMove - 25));
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove - 25);
	wait1Msec(GLOBAL_WAITER + 800);

	MoveLineStop(Right, powerToMove - 20);

	wait1Msec(GLOBAL_WAITER_LONG);

	// wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove); //move forward to grab SECOND MG
	wait1Msec(GLOBAL_WAITER + 600);
	MoveHelper(0); //move forward to grab MG

	moveMobileLiftUpAndStop(); 			//pick up SECOND MG
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveLineStop(Right, FULLPOWER);
	wait1Msec(500);
	TurnGyro(90, turnPower); // 125, 50
	wait1Msec(500);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(500, 110);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(50);
	wait1Msec(400);
	MoveHelper(0);
	moveMobileLiftDownAndStop(false); // Deploy SECOND MG
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);
}

void programmingSkill_2() {
	int powerToMove = 70;
	int turnPower = 125;
	int powerToTurn = 65;
	moveMobileLiftDownToPickUpAuton();
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	moveMobileLiftUpAndStop(); // Pick up FIRST MG
	wait1Msec(GLOBAL_WAITER_LONG);

	moveLiftDownAndHold(MIN_LIFT, powerliftDown);
	wait1Msec(GLOBAL_WAITER);

	openClaw(powerClawOpen); //for one cone only!
	wait1Msec(400); //for one cone only!
	stopClaw(); //for one cone only!
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, -FULLPOWER);
	wait1Msec(GLOBAL_WAITER + 200);
	TurnLine(Back, -(powerToTurn));
	wait1Msec(GLOBAL_WAITER_LONG);
	TurnLine(Left, -(powerToTurn - 22));
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveHelper(30);
	wait1Msec(GLOBAL_WAITER + 100);
	moveMobileLiftDownToPickUpAuton(); // deploy FIRST MG
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveIEM(150, -80);
	wait1Msec(GLOBAL_WAITER);
	TurnLine(Back, (powerToTurn - 22)); //turn right 180"
	wait1Msec(GLOBAL_WAITER_LONG);
	TurnLine(Left, (powerToTurn - 25));  //align with SECOND MG
	wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Left, -(powerToTurn - 32));  //align with SECOND MG
	// wait1Msec(GLOBAL_WAITER_LONG);
	// TurnLine(Right, -(powerToTurn - 22));  //align with SECOND MG
	// wait1Msec(GLOBAL_WAITER_LONG);
	HitBarAndAlign();
	MoveHelper(-(powerToMove - 25));
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveHelper(powerToMove - 25);
	wait1Msec(GLOBAL_WAITER + 800);

	MoveLineStop(Right, powerToMove - 20);

	wait1Msec(GLOBAL_WAITER_LONG);

	// wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove); //move forward to grab SECOND MG
	wait1Msec(GLOBAL_WAITER + 600);
	MoveHelper(0); //move forward to grab MG

	moveMobileLiftUpAndStop(); 			//pick up SECOND MG
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveLineStop(Right, FULLPOWER);
	wait1Msec(500);
	TurnGyro(90, turnPower); // 125, 50
	wait1Msec(500);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(500, 110);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(50);
	wait1Msec(400);
	MoveHelper(0);
	moveMobileLiftDownAndStop(false); // Deploy SECOND MG
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(250 + 100, -100);
	wait1Msec(GLOBAL_WAITER);
	moveMobileLiftUpAndStop();
	wait1Msec(GLOBAL_WAITER_LONG);
	moveLiftDownAndStop(LIFT_PICK_MOBILE, powerliftDown); // LIFT DOWN
	wait1Msec(GLOBAL_WAITER);

	MoveLineStop(Left, powerToMove - 45); //move a bit forward
	wait1Msec(GLOBAL_WAITER + 200);
	MoveLineStop(Left, -(powerToMove - 45)); //move a bit backward
	wait1Msec(GLOBAL_WAITER + 200);
	TurnLine(Back, -(powerToTurn - 20));  //turn left
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(90, -80); //move back
	wait1Msec(GLOBAL_WAITER + 200);
	TurnLine(Left, -(powerToTurn - 30));  //align with bar
	HitBarAndAlign();

	TurnGyro(25, -100); // Make thing similar to the starting position.
	moveMobileLiftDownToPickUpAuton(); //deploy for THIRD mobile goal
	wait1Msec(GLOBAL_WAITER);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	moveMobileLiftUpAndStop();  // Pick up THIRD mobile goal
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveLineStop(Back, -(FULLPOWER-20));
	wait1Msec(GLOBAL_WAITER_LONG + 300);
	TurnLine(Back, -(powerToTurn));
	wait1Msec(GLOBAL_WAITER_LONG);
	TurnLine(Right, -(powerToTurn + 10));
	wait1Msec(GLOBAL_WAITER_LONG);
	TurnGyro(25 - 25, 80);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(40);
	wait1Msec(GLOBAL_WAITER + 100);
	moveMobileLiftDownToPickUpAuton(); // deploy THIRD MG
	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER_LONG);
	MoveIEM(150, -80);
	wait1Msec(GLOBAL_WAITER);
	TurnLine(Back, (powerToTurn)); //turn right 180"
	wait1Msec(GLOBAL_WAITER_LONG);
	TurnLine(Left, (powerToTurn));  //align with FOURTH MG
	wait1Msec(GLOBAL_WAITER_LONG + 100);
	TurnLine(Left, -(powerToTurn + 60)); //turn back to match
	wait1Msec(GLOBAL_WAITER_LONG + 200);
	HitBarAndAlign();
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(powerToMove - 25);
	wait1Msec(GLOBAL_WAITER + 800);

	MoveLineStop(Right, powerToMove - 20);

	wait1Msec(GLOBAL_WAITER_LONG);

	// wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove); //move forward to grab FOURTH MG
	wait1Msec(GLOBAL_WAITER + 600);
	MoveHelper(0); //move forward to grab MG

	moveMobileLiftUpAndStop(); 			//pick up FOURTH MG
	wait1Msec(GLOBAL_WAITER_LONG);

	MoveLineStop(Right, FULLPOWER);
	wait1Msec(500);
	TurnGyro(70, turnPower); // 125, 50
	wait1Msec(500);
	MoveLineStop(Back, FULLPOWER);
	wait1Msec(GLOBAL_WAITER);
	MoveIEM(500, 110);
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(50);
	wait1Msec(500);
	MoveHelper(0);
	moveMobileLiftDownAndStop(false);  // deploy FOURTH MG
	wait1Msec(GLOBAL_WAITER);
	MoveHelper(-70);
	wait1Msec(GLOBAL_WAITER + 400);
	MoveHelper(0);
	// wait1Msec(GLOBAL_WAITER);
	// MoveIEM(250 + 100, -100);
	// wait1Msec(GLOBAL_WAITER);
	// moveMobileLiftUpAndStop();
	// wait1Msec(GLOBAL_WAITER_LONG);

}

void HitBarAndAlign(){

	//int powerToMove = 70;

	MoveHelper(-60);

	wait1Msec(GLOBAL_WAITER + 600);

	MoveHelper(0);
	wait1Msec(GLOBAL_WAITER + 200);

	// moveMobileLiftDownToPickUpAuton(); //deploy mobile goal lift


	writeDebugStreamLine("8-4");

	// MoveHelper(powerToMove);
	// wait1Msec(800);


	//MoveLineStop(Left, powerToMove - 45); // turn Right

	// MoveLineStop(Right, powerToMove - 30);
	//wait1Msec(GLOBAL_WAITER + 200);


	// TurnLine(Left, -(powerToMove - 30)); // turn left

	// wait1Msec(GLOBAL_WAITER + 200);


	// TurnLine(Left, powerToMove - 30); // turn Right

	// wait1Msec(GLOBAL_WAITER + 200);
}



void autonCone5(bool leftSide) {
	int powerToMove = 70;

	bool ret = autoCommon(2, leftSide);
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER + 200);

	// ret = MoveLine(Back, -powerToMove);
	// if (!ret){
	// 	return;
	// }
	// wait1Msec(500);

	// ret = MoveLineStop(Back, -(powerToMove - 35));
	// if (!ret){
	// 	return;
	// }

	// wait1Msec(GLOBAL_WAITER + 300);

	int turnValue = 870;
	if (leftSide == false) {
		// RIGHT
		TurnGyro(turnValue + 750, powerToMove - 25);
	}
	else {
		// LEFT
		TurnGyro(turnValue + 750, -(powerToMove - 25));
		// ret = TurnLine(Left, -(powerToMove - 20));
		// if (!ret){
		// 	return;
		// }
		// wait1Msec(GLOBAL_WAITER);
		// TurnGyro(500, -50);
	}
	// wait1Msec(GLOBAL_WAITER);
	moveMobileLiftDownToPickUp();
	MoveHelper(-80);
	wait1Msec(560);
	if (leftSide == false) {
		// RIGHT
		wait1Msec(100);
	}

	MoveHelper(0);
	// MoveIEM(300, -80);

}

bool autoCommon(int coneCount, bool leftSide) {

	// coneCount = 1;
	int powerToMove = 70;

	SetInitialPositionAuton();
	//moveLiftUp(20);

	bool ret = MoveLine(Back, powerToMove + 15);
	if (!ret){
		return false;
	}

	// writeDebugStreamLine("1001");
	wait1Msec(300);
	ret = MoveLineStop(Back, powerToMove - 15);
	if (!ret){
		return false;
	}

	// writeDebugStreamLine("1002");
	wait1Msec(GLOBAL_WAITER);
	// writeDebugStreamLine("1003");
	// int target = getLiftPosition() - 50;
	// moveLiftDownAndStop(target, powerliftDown);
	// moveLiftDown(FULLPOWER);
	// wait1Msec(GLOBAL_WAITER);
	// stopLift();
	// writeDebugStreamLine("1004");
	// wait1Msec(100);
	// openClaw(powerClawClose); //for one cone only!

	// coneCount = 2;

	if (coneCount > 1){
		moveMobileLiftUpToStackAuton(FULLPOWER);
		wait1Msec(GLOBAL_WAITER);
		moveLiftDownAndStop(MIN_LIFT, powerliftDown);
		openClaw(powerClawClose); //for one cone only!
		// writeDebugStreamLine("1005");
		// // moveLiftUpAndStop(LIFT_PICK_MOBILE, FULLPOWER);
		// moveLiftUp(FULLPOWER);
		// writeDebugStreamLine("1006");
		// wait1Msec(100);
		// moveMobileLiftUp(FULLPOWER);
		// writeDebugStreamLine("1007");
		// moveLiftUpAndStop(LIFT_PICK_MOBILE, FULLPOWER);
		// stopClaw();
		// wait1Msec(100);
		// writeDebugStreamLine("1008");
		MoveIEM(50 -  10, FULLPOWER);
		wait1Msec(GLOBAL_WAITER);
		stopClaw();
		moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown + 10);

		// moveMobileLiftUpAndStop();
		// writeDebugStreamLine("1009");

		closeClaw(FULLPOWER);
		wait1Msec(GLOBAL_WAITER);
		moveLiftDownAndHold(MIN_LIFT, FULLPOWER);
		wait1Msec(200);

		moveLiftUp(FULLPOWER);
		wait1Msec(200);
		stopLift();

		moveClawLiftUpAndStop(MID_CLAWLIFT, powerClawLiftUp);

		moveLiftDown(FULLPOWER);
		wait1Msec(150);
		stopLift();

		openClaw(powerClawOpen);

		wait1Msec(GLOBAL_WAITER);

		// MoveIEM(60, FULLPOWER);
		// moveLiftUp(FULLPOWER);
		// wait1Msec(200);
		// stopLift();
		// wait1Msec(GLOBAL_WAITER);
		// stopClaw();
		// moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown + 10);
		// moveLiftDown(FULLPOWER);
		// wait1Msec(200);
		// stopLift();

		// closeClaw(FULLPOWER);
		// wait1Msec(GLOBAL_WAITER);
		// moveLiftDownAndHold(MIN_LIFT, FULLPOWER);
		// wait1Msec(200);
	}
	else
	{
		// moveLiftUp(FULLPOWER);
		// wait1Msec(GLOBAL_WAITER);
		// stopLift();

		moveMobileLiftUpToStackAuton(FULLPOWER);
		wait1Msec(GLOBAL_WAITER);
		moveLiftDownAndStop(MIN_LIFT, powerliftDown);
		openClaw(powerClawClose); //for one cone only!
		wait1Msec(300);
		stopClaw();

		moveLiftDownAndStop(MIN_LIFT, powerliftDown);
		moveClawLiftUpAndStop(MID_CLAWLIFT, powerClawLiftUp);
	}


	stopLift();

	MoveHelper(-powerToMove);
	wait1Msec(300);
	// writeDebugStreamLine("1003");

	stopClaw(); //for one cone only!

	ret = MoveLine(Back, -powerToMove);
	if (!ret){
		return false;
	}

	// writeDebugStreamLine("1004");
	wait1Msec(500);
	ret = MoveLineStop(Back, -(powerToMove - 10));
	if (!ret){
		return false;
	}

	// writeDebugStreamLine("1005");

	// // wait1Msec(100);
	// ret = MoveLineStop(Left, -30);
	// if (!ret){
	// 	return;
	// }

	return true;
}

void autonCone20(int coneCount, bool leftSide) {
	int powerToMove = 70;

	bool ret = autoCommon(coneCount, leftSide);
	if (!ret){
		return;
	}
	wait1Msec(GLOBAL_WAITER + 100);

	if (leftSide) {
		ret = TurnLine(Back, -(powerToMove-25));
		if (!ret){
			return;
		}

		// writeDebugStreamLine("1006");

		wait1Msec(GLOBAL_WAITER + 100);
		ret = TurnLine(Back, (powerToMove - 35));
		if (!ret){
			return;
		}
	}
	else {
		ret = TurnLine(Back, (powerToMove-25));
		if (!ret){
			return;
		}

		// writeDebugStreamLine("1006");

		wait1Msec(GLOBAL_WAITER + 100);
		ret = TurnLine(Back, -(powerToMove - 35));
		if (!ret){
			return;
		}
	}
	wait1Msec(GLOBAL_WAITER);

	// writeDebugStreamLine("1007");

	// MoveIEM(490 - 50, -(powerToMove + 10));
	if (leftSide) {
		MoveIEM(490 - 50, -(powerToMove + 10)); //this is just right at home, might need to increase at competition
	}
	else {
		MoveIEM(490 + 50, -(powerToMove + 10));
	}

	// writeDebugStreamLine("1008");

	wait1Msec(GLOBAL_WAITER + 100);

	bool useGyro = true;

	if (useGyro){
		if (leftSide) {
			ClearEncoder();
			// TurnGyroLeft(90);
			TurnGyro(870 - 100, -(powerToMove - 25));
			// wait1Msec(GLOBAL_WAITER + 200);
			// int delta = SensorValue[GyroPosition] - turnValue;
			// TurnGyro(delta, powerToMove);
			writeDebugStreamLine("Gyro: %d", SensorValue[GyroPosition]);
			writeDebugStreamLine("Encoder: %d", nMotorEncoder[wheelRight]);
		}
		else {
			ClearEncoder();
			// TurnGyroLeft(90);
			TurnGyro(870 - 100, (powerToMove - 25));
			// wait1Msec(GLOBAL_WAITER + 200);
			// int delta = SensorValue[GyroPosition] - turnValue;
			// TurnGyro(delta, powerToMove);
			writeDebugStreamLine("Gyro: %d", SensorValue[GyroPosition]);
			writeDebugStreamLine("Encoder: %d", nMotorEncoder[wheelRight]);
		}
	}
	else{
		SensorValue[GyroPosition] = 0;
		ret = TurnLine(Left, -(powerToMove - 25)); //might need to increase at competition

		writeDebugStreamLine("Gyro: %d", SensorValue[GyroPosition]);

		// ret = TurnLine(Left, -(powerToMove-10 - 5));
		if (!ret){
			moveMobileLiftDownToPickUp();
			MoveHelper(-80);
			wait1Msec(150);
			stopAll();

			MoveHelper(0);

			return;
		}
	}

	// writeDebugStreamLine("1008-1");

	int mPower = 120;
	int mDistaince = 500;

	// if (coneCount > 1) {
	// 	mPower = FULLPOWER;
	// 	mDistaince = 500;
	// }

	// wait1Msec(GLOBAL_WAITER + 50); don't need this because of moveLiftUp

	moveLiftUp(FULLPOWER);
	wait1Msec(200);
	stopLift();

	MoveIEM(mDistaince, mPower);
	// wait1Msec(GLOBAL_WAITER);
	// writeDebugStreamLine("1009");
	moveMobileLiftDown(FULLPOWER);
	wait1Msec(400);
	stopMobileLift();

	// moveMobileLiftDownAndStopAuton(HIGHGOAL_MOBILELIFT + 200);

	// writeDebugStreamLine("1010");
	MoveHelper(75);
	wait1Msec(200);
	MoveHelper(50);
	wait1Msec(100);
	MoveHelper(0);

	// moveMobileLiftUpAuton();
	// wait1Msec(GLOBAL_WAITER);
	// MoveIEM(250, -100);
	MoveHelper(-80);
	wait1Msec(400);
	stopAll();

	MoveHelper(0);
}

void autonStationary(bool leftSide) {
	int powerToMove = 70;

	ClearEncoder();

	closeClaw(40);
	// wait1Msec(300);
	bool ret = moveLiftUpAndHold(LIFT_PICK_MOBILE - 50, FULLPOWER);
	if (!ret){
		return;
	}

	holdLift();

	ret = MoveLine(Back, powerToMove - 40);
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER + 200);

	ret = MoveLineStop(Back, -(powerToMove - 50));
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER);

	moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown / 2);

	wait1Msec(GLOBAL_WAITER);

	openClaw(powerClawOpen);
	wait1Msec(100);
	openClaw(powerClawClose);

	PushORUpCone(false);

	stopClaw();

	ret = MoveLineStop(Right, -(powerToMove - 44));
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER);

	int turnValue = 870 - 100;
	ClearEncoder();
	TurnGyro(turnValue, (powerToMove - 25));

	wait1Msec(GLOBAL_WAITER + 100);

	moveMobileLiftDownToPickUpAuton(); //deploy for THIRD mobile goal

	wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove + 15);
	wait1Msec(GLOBAL_WAITER + 700);

	// bool ret = MoveLine(Back, powerToMove + 15);
	// if (!ret){
	// 	return;
	// }

	// writeDebugStreamLine("1001");
	wait1Msec(300);
	ret = MoveLineStop(Back, powerToMove - 15 - 5);
	if (!ret){
		return;
	}


	// wait1Msec(GLOBAL_WAITER);
	//TurnGyro(750, 70);
	//wait1Msec(GLOBAL_WAITER + 200);
	//MoveAccel(700, 50);
	stopAll();
}

void defensive(bool leftSide) {
	int powerToMove = 70;

	ClearEncoder();

	closeClaw(40);
	// wait1Msec(300);
	bool ret = moveLiftUpAndHold(LIFT_PICK_MOBILE - 50, FULLPOWER);
	if (!ret){
		return;
	}

	holdLift();

	ret = MoveLine(Back, powerToMove - 40);
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER + 200);

	ret = MoveLineStop(Back, -(powerToMove - 50));
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER);

	moveClawLiftDownAndStop(MIN_CLAWLIFT, powerClawLiftDown / 2);

	wait1Msec(GLOBAL_WAITER);

	openClaw(powerClawOpen);
	wait1Msec(100);
	openClaw(powerClawClose);

	PushORUpCone(false);

	stopClaw();

	ret = MoveLineStop(Right, -(powerToMove - 44));
	if (!ret){
		return;
	}

	wait1Msec(GLOBAL_WAITER);

	int turnValue = 870 - 100;
	ClearEncoder();
	TurnGyro(turnValue, (powerToMove - 25));

	wait1Msec(GLOBAL_WAITER + 100);

	moveMobileLiftDownToPickUpAuton(); //deploy for THIRD mobile goal

	wait1Msec(GLOBAL_WAITER);

	MoveHelper(powerToMove + 15);
	wait1Msec(GLOBAL_WAITER + 700);

	// bool ret = MoveLine(Back, powerToMove + 15);
	// if (!ret){
	// 	return;
	// }

	// writeDebugStreamLine("1001");
	wait1Msec(300);
	ret = MoveLineStop(Back, powerToMove - 15 - 5);
	if (!ret){
		return;
	}


	// wait1Msec(GLOBAL_WAITER);
	//TurnGyro(750, 70);
	//wait1Msec(GLOBAL_WAITER + 200);
	//MoveAccel(700, 50);
	stopAll();
}

// END LC