PASTEPPP

#include "robot-config.h"
#include <cmath>
#include "math.h"

enum class TurnDir { Left, Right };
enum class StartSide { Blue, Red };
enum class AutoRoutine { Routine1, Routine2 };

int count = 0;
int target = 0;
bool driveMode = false;
int PreviousErrorLeft = 0;
    int PreviousErrorRight = 0;
vex::digital_in AutoStartSide = vex::digital_in(Brain.ThreeWirePort.B);
vex::digital_in Routine = vex::digital_in(Brain.ThreeWirePort.C);

vex::competition    Competition;

/*---------------------------------------------------------------------------*/
void pre_auton(void) {
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/


void RightMotorFB(double speed) {
    RightMotorF.spin(directionType::fwd,speed,velocityUnits::pct);
    RightMotorB.spin(directionType::fwd,speed,velocityUnits::pct);
}

void LeftMotorFB(double speed) {
    LeftMotorF.spin(directionType::fwd,speed,velocityUnits::pct);
    LeftMotorB.spin(directionType::fwd,speed,velocityUnits::pct);
}

 void slop(int distance) {
     if (distance < 0) {
         LeftMotorFB(-40);
         vex::task::sleep(35);
     }
 }


    const int accel_step = 9;
const int deccel_step = 100; // no decel slew
static int leftSpeed = 0;
static int rightSpeed = 0;

void LeftMotorFB(int leftTarget){
  int step;

  if(abs(leftSpeed) < abs(leftTarget))
    step = accel_step;
  else
    step = deccel_step;

  if(leftTarget > leftSpeed + step)
    leftSpeed += step;
  else if(leftTarget < leftSpeed - step)
    leftSpeed -= step;
  else
    leftSpeed = leftTarget;

  LeftMotorFB(leftSpeed);
}
//slew control
void RightMotorFB(int rightTarget){
  int step;

  if(abs(rightSpeed) < abs(rightTarget))
    step = accel_step;
  else
    step = deccel_step;

  if(rightTarget > rightSpeed + step)
    rightSpeed += step;
  else if(rightTarget < rightSpeed - step)
    rightSpeed -= step;
  else
    rightSpeed = rightTarget;

  RightMotorFB(rightSpeed);
}

int drivePos() {
     return (LeftMotorF.rotation(rotationUnits::deg) + RightMotorF.rotation(rotationUnits::deg))/2;
}
bool isDriving() {
    return(LeftMotorF.isSpinning() || LeftMotorB.isSpinning() || RightMotorF.isSpinning() || RightMotorB.isSpinning());

}

void PidBasicAsync(int distance) {
    slop(distance);
    count = 0;
    target = distance;
    driveMode = true;
    LeftMotorF.resetRotation();
    RightMotorF.resetRotation();
      LeftMotorB.resetRotation();
    RightMotorB.resetRotation();
    PreviousErrorLeft = 0;
    PreviousErrorRight = 0;
}

void PidBasic (int distance) {
    if (driveMode) {

    double Kp = 0.18;///0.2 or .215
    double Kd = 0.5;//0.28;///0.275 or .0325


    int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    int ErrorLeft = distance - Leftdegrees;
    int ErrorRight = distance - Rightdegrees;



         Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    ErrorLeft = distance - Leftdegrees;
    ErrorRight = distance - Rightdegrees;

        int LeftsideProportionalError = Kp * ErrorLeft;
        int RightsideProportionalError = Kp * ErrorRight;

        double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
        double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);

        LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
        RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);

        PreviousErrorLeft = ErrorLeft;
        PreviousErrorRight = ErrorRight;
        /*if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
		    break;
	    }*/
        if (!isDriving()) {
            count++;
        }
        if (count > 30) {
            driveMode = false;
        }
        vex::task::sleep(30);
    }
}

void Turn (int distance) {
    LeftMotorF.resetRotation();
    RightMotorF.resetRotation();
      LeftMotorB.resetRotation();
    RightMotorB.resetRotation();
    double Kp = .3;
    double Kd = .35;

    int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

    int ErrorLeft = -1*Leftdegrees - distance;
    int ErrorRight = distance - Rightdegrees;

    int PreviousErrorLeft = 0;
    int PreviousErrorRight = 0;

    while(1) {
         Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
    Rightdegrees=RightMotorF.rotation(rotationUnits::deg);

     ErrorLeft = -1*Leftdegrees - distance;
     ErrorRight = distance - Rightdegrees;

        int LeftsideProportionalError = Kp * ErrorLeft;
        int RightsideProportionalError = Kp * ErrorRight;

        double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
        double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);

        LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
        RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);

        PreviousErrorLeft = ErrorLeft;
        PreviousErrorRight = ErrorRight;
        if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
		    break;
	    }
        vex::task::sleep(30);
    }
}
/////////////////////////////////////////////////////////////////////////////////////////////

void StopMotor(vex::brakeType type = brakeType::coast){
    LeftMotorF.stop(type);
    RightMotorF.stop(type);
    LeftMotorB.stop(type);
    RightMotorB.stop(type);
}
void SetMotorBrakingType(brakeType type = brakeType::coast) {
    LeftMotorF.setStopping(type);
    RightMotorF.setStopping(type);
    LeftMotorB.setStopping(type);
    RightMotorB.setStopping(type);
}
int AutoBallIntakeFlipCap() {
    int timeout = 4000;
    Scooper.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::rev, 50, velocityUnits::pct);

    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    Scooper.stop(brakeType::coast);

    return(0);
}
int AutoBallIntakeFast() {
    int timeout = 4000;
    Scooper.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::fwd, 100, velocityUnits::pct);

    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }
    Scooper.stop(brakeType::coast);
    return(0);
}
int AutoBallIntakeTakeBall(int timeout = 5000) {

    Scooper.setStopping(brakeType::coast);
    int sleepTimeout = 20;
    Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
    while (timeout > 0) {
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    Scooper.stop(brakeType::coast);

    return(0);
}
void Drive(double numOfRevs, int velocity = 50, bool fwd = true, int timeout = 5000)
{
    Brain.Screen.printAt(1, 40, "numofrevs: %f, vel: %f", numOfRevs, velocity);
    SetMotorBrakingType();

    int sleepTimeout = 20;

    if (fwd == false) {
        numOfRevs = numOfRevs * -1;
    }

    LeftMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    RightMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    LeftMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
    RightMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);

    while(LeftMotorF.isSpinning()||RightMotorF.isSpinning()||
          LeftMotorB.isSpinning()||RightMotorB.isSpinning()) {

        if (timeout <= 0) {
            break;
        }
        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    StopMotor();
}

void DriveInCM(double distanceInCM, int velocity, bool fwd = true, int timeout = 5000) {
  //  double gearRatio = 1.67;
  //  double wheelDiameter = 10.16;
  //  double circumference = wheelDiameter * M_PI;
  //  double numOfRevs = (distanceInCM)/ (circumference * gearRatio);
    double numOfRevs = distanceInCM/53.2;
    Drive(numOfRevs, velocity, fwd, timeout);
}

void Turn(double degrees, TurnDir turnDir, int velocity = 25, int timeout = 5000){
     int sleepTimeout = 10;

    SetMotorBrakingType();
    LeftMotorF.resetRotation();
    RightMotorF.resetRotation();

    if (turnDir == TurnDir::Right) {
        LeftMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
        LeftMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);

        RightMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
        RightMotorB.spin(directionType::rev, velocity, velocityUnits::pct);
    }
    else
    {
        LeftMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
        LeftMotorB.spin(directionType::rev, velocity, velocityUnits::pct);

        RightMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
        RightMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);
    }
    while(true) {

        double flmDeg = std::abs(LeftMotorF.rotation(rotationUnits::deg));
        double frmDeg = std::abs(RightMotorF.rotation(rotationUnits::deg));
        Brain.Screen.clearScreen();
        Brain.Screen.printAt(1, 20, "FL: %f FR: %f", flmDeg, frmDeg);

        if (frmDeg > degrees) {
            Brain.Screen.printAt(1, 40, "Deg Break: FL: %f FR: %f", flmDeg, frmDeg);
            break;
        }

        if (timeout <= 0) {
            Brain.Screen.printAt(1, 40, "Timeout Break: FL: %f FR: %f", flmDeg, frmDeg);
            break;
        }

        task::sleep(sleepTimeout);
        timeout = timeout - sleepTimeout;
    }

    StopMotor(brakeType::coast);
}

void TurnRight90Degrees()
{
    Turn(85, TurnDir::Right);
}

void TurnLeft90Degrees()
{
    Turn(85, TurnDir::Left);
}

void CatapultLaunch() {

    int timeout = 5000;

    CatapultMotor.setStopping(brakeType::coast);
    CatapultMotor2.setStopping(brakeType::coast);

    int sleepTimeout = 30;
    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float catPos = P1.value(rotationUnits::deg);

        Brain.Screen.printAt(1, 40, "In launch CatPos: %f", catPos);

        if (catPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // Lowest position for launch
        if (catPos <= 54)
        {
            Brain.Screen.printAt(1, 80, "Launched: %f", catPos);
            break;
        }

        // In case lowest position did not reach and catapult already launched.
        // This is failsafe

        if (catPos >= 141)
        {
            break;
        }

        CatapultMotor.spin(directionType::fwd, 100, velocityUnits::pct);
         CatapultMotor2.spin(directionType::fwd, 100, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;

        Brain.Screen.printAt(1, 20, "Timeout: %d", timeout);
    }

    CatapultMotor.stop(brakeType::coast);
    CatapultMotor2.stop(brakeType::coast);
}

void CatapultLoadPosition() {

    int timeout = 5000;

    CatapultMotor.setStopping(brakeType::coast);
      CatapultMotor2.setStopping(brakeType::coast);


    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float catPos = P1.value(rotationUnits::deg);

        Brain.Screen.printAt(1, 40, "In load CatPos: %f", catPos);

        if (catPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // load position
        if (catPos <= 72)
        {
            break;
        }

        CatapultMotor.spin(directionType::fwd, 100, velocityUnits::pct);
        CatapultMotor2.spin(directionType::fwd, 100, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(1, 20, "Timeout: %d", timeout);
    }

    CatapultMotor.stop(brakeType::coast);
    CatapultMotor2.stop(brakeType::coast);
}

/////////////////////////////////////////////////////////////////////////////////
void LiftPosition1 () {

    int timeout = 5000;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;
    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (LiftPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // Lowest position for launch
        if (LiftPos >= 245)
        {
            Brain.Screen.printAt(2, 80, "Stopped: %f", LiftPos);
            break;
        }

        Arm2000.spin(directionType::fwd, 60, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;

        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::hold);
}

void LiftPosition2() {

    int timeout = 5000;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (LiftPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // load position
        if (LiftPos <= 76)
        {
            break;
        }

        Arm2000.spin(directionType::rev, 80, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::coast);
}

void LiftPosition3() {
    int timeout = 5000;

    Arm2000.setStopping(brakeType::coast);

    int sleepTimeout = 30;

    while (timeout > 0) {

        Brain.Screen.clearScreen();

        float LiftPos = P2.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (LiftPos == 0)
        {
            task::sleep(sleepTimeout);
            continue;
        }

        // load position
        if (LiftPos >= 210)
        {
            break;
        }

        Arm2000.spin(directionType::fwd, 80, velocityUnits::pct);

        task::sleep(sleepTimeout);

        timeout = timeout - sleepTimeout;
        Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
    }

    Arm2000.stop(brakeType::coast);
}



void AutoRoutine1(StartSide startSide)
{

    //
 DriveInCM(5, 33);
    PidBasicAsync(700);

          while(driveMode) {

              PidBasic(target);
             if (drivePos() > 400) {
                 Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
             }


          }

         DriveInCM(0, 30);


          vex::task::sleep(200);




    PidBasicAsync(-680);

         DriveInCM(-1, 10);


     DriveInCM(0, 5);

    vex::task::sleep(200);


          while(driveMode) {

              PidBasic(target);
             if (drivePos() < -400) {
                 Scooper.spin(directionType::fwd, 0, velocityUnits::pct);
             }


          }


          vex::task::sleep(600);

      DriveInCM(2, 5);

            PidBasicAsync(80);

      DriveInCM(0, 5);

        while(driveMode) {

              PidBasic(target);


          }
          vex::task::sleep(600);


               if  ( startSide == StartSide::Blue)
    {
        Turn(-128.5);////126.5
    }
    else
    {
         Turn(128.5);////126.5
    }


            LeftMotorF.stop(brakeType::brake);
    RightMotorB.stop(brakeType::brake);
       LeftMotorB.stop(brakeType::brake);
    RightMotorF.stop(brakeType::brake);




          vex::task::sleep(200);



       DriveInCM(0, 10);
            vex::task::sleep(500);
          PidBasicAsync(305);


          while(driveMode) {

              PidBasic(target);


          }

    vex::task::sleep(200);
        CatapultLaunch();


               if  ( startSide == StartSide::Blue)
    {
        Turn(-21);
    }
    else
    {
         Turn(21);
    }

           DriveInCM(0, 10);

        vex::task::sleep(10);



    PidBasicAsync(700);

       DriveInCM(0, 10);

    {
    while(driveMode) {
    int catPos = P1.value(rotationUnits::deg);
        Brain.Screen.printAt(5, 20, "catpos: %d degrees", catPos);
              PidBasic(target);

              if (drivePos() > 300) {
                 Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
              }

          }
    }

    vex::task::sleep(200);



     while(driveMode) {

              PidBasic(target);


          }



    PidBasicAsync(0);
          while(driveMode) {

              PidBasic(target);

              if (drivePos() > 100) {
                 driveMode = false;
             }
          }

     LeftMotorF.stop(brakeType::coast);
    RightMotorB.stop(brakeType::coast);
       LeftMotorB.stop(brakeType::coast);
    RightMotorF.stop(brakeType::coast);


}


void CheckCatPotValues() {

    while (1)
    {
        Brain.Screen.clearScreen();
        Brain.Screen.printAt(1, 80, "CheckCatPot");
        float catPos = P1.value(rotationUnits::deg);
        Brain.Screen.printAt(1, 20, "rotation: %f degrees", catPos);

        //Sleep the task for a short amount of time to prevent wasted resources.
        task::sleep(20);
    }
}


void CheckLiftPotValues() {

    while (1)
    {
        Brain.Screen.clearScreen();
        Brain.Screen.printAt(1, 80, "CheckLiftPot");
        float LiftPos = P2.value(rotationUnits::deg);
        Brain.Screen.printAt(1, 20, "rotation: %f degrees", LiftPos);

        //Sleep the task for a short amount of time to prevent wasted resources.
        task::sleep(20);
    }
}
///////////////////////////
void AutoRoutine2(StartSide startSide)
{
   DriveInCM(8, 33);

    PidBasicAsync(700);
          while(driveMode) {

              PidBasic(target);
             if (drivePos() > 400) {
                 Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
             }


          }
         DriveInCM(1, 33);
          vex::task::sleep(200);

          PidBasicAsync(-80);
          while(driveMode) {

              PidBasic(target);
             if (drivePos() < -400) {
                 Scooper.spin(directionType::fwd, 0, velocityUnits::pct);
             }


          }

          vex::task::sleep(300);

        if  ( startSide == StartSide::Blue)
    {
        Turn(-123);
    }
    else
    {
         Turn(123);
    }

          vex::task::sleep(200);
          PidBasicAsync(130);
          while(driveMode) {

              PidBasic(target);


          }



     if  ( startSide == StartSide::Blue)
    {
        Turn(20);
    }
    else
    {
         Turn(-20);
    }
    PidBasicAsync(-100);
          while(driveMode) {

              PidBasic(target);


          }



    vex::task::sleep(200);


    PidBasicAsync(1100);
          while(driveMode) {

              PidBasic(target);

              if (drivePos() > 1000) {
                 driveMode = false;
             }
          }

     LeftMotorF.stop(brakeType::brake);
    RightMotorB.stop(brakeType::brake);
       LeftMotorB.stop(brakeType::brake);
    RightMotorF.stop(brakeType::brake);




}

void autonomous(void) {
    int startSideValue = AutoStartSide.value();
    StartSide startSide;

    if (startSideValue == 1)
    {
        startSide = StartSide::Blue;
    }
    else
    {
        startSide = StartSide::Red;
    }

    int routineValue = Routine.value();
    AutoRoutine routine;

    // no pin
    if (routineValue == 1)
    {

        routine = AutoRoutine::Routine1;
    }
    else
    {

        routine = AutoRoutine::Routine2;
    }

    if (routine == AutoRoutine::Routine1)
    {

        AutoRoutine1(startSide);
    }
    else
    {

        AutoRoutine2(startSide);
    }

    return;
}

int DriveTask() {
    while (true) {
        if (driveMode) {
            PidBasic(target);
        }
    }
}

int CatapultTask()
{
    while (true) {
        Brain.Screen.clearScreen();
        float catPos = P1.value(rotationUnits::deg);

        Brain.Screen.printAt(1, 40, "CatPos: %f", catPos);

        if (Controller1.ButtonR1.pressing()) {
            // Load
            CatapultLoadPosition();
        }
        else if (Controller1.ButtonR2.pressing()) {
            // Launch
            CatapultLaunch();
        }

        task::sleep(30);
    }
    return (0);
}

int LiftTask()
{
    while (true) {
        Brain.Screen.clearScreen();
        float LiftPos = P1.value(rotationUnits::deg);

        Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);

        if (Controller1.ButtonY.pressing()) {
            // Load
           LiftPosition1();
        }
        else if (Controller1.ButtonA.pressing()) {
            // Launch
            LiftPosition2();
        }

         else if (Controller1.ButtonB.pressing()) {
            // Launch
            LiftPosition3();
        }

        task::sleep(30);
    }
    return (0);
}
/*----------------------------------------------------------------------------*/
/*                                                                            */
/*                              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.    */
/*----------------------------------------------------------------------------*/
void usercontrol(void) {

//   CheckLiftPotValues();

     task CallLiftTask(LiftTask);
    task CallCatapultTask(CatapultTask);


    while (true) {

        RightMotorF.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
        RightMotorB.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
        LeftMotorF.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);
        LeftMotorB.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);


            LeftMotorF.setStopping(brakeType::coast);
    RightMotorB.setStopping(brakeType::coast);
       LeftMotorB.setStopping(brakeType::coast);
    RightMotorF.setStopping(brakeType::coast);



        if (Controller1.ButtonL2.pressing()) {
            //If button up is pressed...Spin the arm motor forward
            Scooper.spin(vex::directionType::fwd, 100, velocityUnits::pct);
        }
        else if (Controller1.ButtonL1.pressing()) { //If the down button is pressed...
            Scooper.spin(vex::directionType::rev, 100, velocityUnits::pct);
        }else if (Controller2.ButtonUp.pressing()) {
            //If button up is pressed...Spin the arm motor forward
            Scooper.spin(vex::directionType::fwd, 100, velocityUnits::pct);
        }
        else if (Controller2.ButtonLeft.pressing()) { //If the down button is pressed...
            Scooper.spin(vex::directionType::rev, 100, velocityUnits::pct);
        }
        else { //If the the up or down button is not pressed...
            Scooper.stop(brakeType::coast);
        }


/*
        if (Controller1.ButtonB.pressing()) {
            //If button up is pressed...Spin the arm motor forward
            Arm2000.spin(vex::directionType::fwd, 100, velocityUnits::pct);
        }
        else if (Controller1.ButtonA.pressing()) { //If the down button is pressed...
            Arm2000.spin(vex::directionType::rev, 100, velocityUnits::pct);
        }
        else { //If the the up or down button is not pressed...
            Arm2000.stop(brakeType::coast);
        }
       */


        }
    }


int main() {

    Brain.Screen.clearScreen();
    float LiftPos = P2.value(rotationUnits::deg);
    Brain.Screen.printAt(1, 40, "LiftPos: %e", LiftPos);


    Brain.Screen.clearScreen();
    float catPos = P1.value(rotationUnits::deg);
    Brain.Screen.printAt(1, 40, "CatPos: %f", catPos);


    pre_auton();
    Competition.autonomous(autonomous);
    Competition.drivercontrol(usercontrol);

    //Prevent main from exiting with an infinite loop.

    while (1) {

       vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
    }
}