//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void LiftPower(int Power);


bool LiftAuto = false;
bool LiftTargetReached = false;
int Tolerance = 65;
float ToleranceMultiplier = 2.5;
int LiftTarget = 0;
int LiftPosition = 0;
int MotorPower = 0;
int error = 0;
float LiftP = 2;

const int LiftTargetGround = 0;
const int LiftTargetBarrier = 660;
const int LiftTargetScore = 1420;

void LiftPower(int Power)
{
	int OutPower;
	if (Power > 127) OutPower = 127;
	else if (Power < -127) OutPower = -127;
	else OutPower = Power;

  motor[LiftLeft] = OutPower;
  motor[LiftRight] = OutPower;
  motor[LiftLeft2] = OutPower;
  motor[LiftRight2] = OutPower;
}


//===============================================| Arm |==============================================
task Arm()
{
	//nMotorEncoder[LIft] = 0;

	while(true)
  {
	  LiftPosition = SensorValue[LIft];
    error = abs(LiftPosition - LiftTarget);

    if (LiftAuto)
    {
      LiftTargetReached = false;

      if (vexRT[Btn8DXmtr2])       //Move to ground
	    {
	      LiftTarget = LiftTargetGround;
	    }
	    else if (vexRT[Btn8RXmtr2]) //Move to barrier
	    {
	      LiftTarget = LiftTargetBarrier;
	    }
	    else if (vexRT[Btn8UXmtr2]) //Move to Score
	    {
	      LiftTarget = LiftTargetScore;
	    }

	    if (abs(vexRT[Ch2Xmtr2]) > 10)
	    {
	      LiftAuto = false;
	    }

	    //PID loop
      if ((SensorValue[LIft] - LiftTarget) > (Tolerance * ToleranceMultiplier))
      {
        MotorPower = -(error * LiftP);
      }
      else if ((SensorValue[LIft] - LiftTarget) > (Tolerance))
      {
        MotorPower = -(error * LiftP / 2);
      }else if ( abs(SensorValue[LIft] - LiftTarget) > (Tolerance * ToleranceMultiplier))
      {
        MotorPower = (error * LiftP);
      }else if ( abs(SensorValue[LIft] - LiftTarget) > (Tolerance))
      {
        MotorPower = (error * LiftP / 3);
      }else
      {
        MotorPower = 0;
        LiftTargetReached = true;
      }

      //Set actual motor values
      LiftPower(MotorPower);

	  }//End if LiftAuto
	  else
	  {
	    if ((vexRT[Btn8DXmtr2]) || (vexRT[Btn8RXmtr2]) || (vexRT[Btn8UXmtr2]))
	    {
	      LiftAuto = true;
	    }

	    LiftPower(vexRT[Ch2Xmtr2]);

	  }//End LiftAuto if (else)
  }//End while loop
}//End task "Arm"
//====================================================================================================