P loop help

Hi, I am working on coding a p loop for an auto stacking tray. It works perfectly on the way up, but I can not get it to come back down. Does anyone know what I did wrong?

void usercontrol(void) {
  while (1) {
    if (Controller1.ButtonL1.pressing()){
     double Kp; //value that converts degrees to motor speed
     double error; //difference between where we want to be and where we are
     double target; //where we want to be
     double speed; //motor speed found using loop
     double speed1; //motor speed plus constant to keep it moving 

      Kp = 0.03; //defines constant value
      target = 3100; // sets target
     error = target - Motor1.position(degrees); // error equals goal minus where we are
     speed = error * Kp; //converts error into motor speed
     speed1 = speed + 10; //adds minumum to speed
    if(speed1 > 75){ //prevents speed from going over desired value
      speed1 = 75;

    if(speed1 < -75){ //prevents speed from going under value
      speed1 = -75;

    Motor1.spin(vex::directionType::fwd, speed1, vex::velocityUnits::pct); // motor spins at speed1
    wait(20, msec);//wait to prevent wasted resources
    else if (Controller1.ButtonL2.pressing()){
      Motor1.rotateTo(0, rotationUnits::deg, 100, velocityUnits::pct); //if button is pressed tray auto reset

   wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.


Thank you

1 Like

It looks like you have an infinite loop within your button L1 pressed conditional, the code is getting stuck there and can’t break out of that loop. You’ll probably want to structure the PID calculations a bit differently so you are able to just set the target values in the conditional and then run the motors to those values rather than doing so directly. I’d recommend making a more generic PID class or function to take in and work with values, that’s the route I went with my code and it works pretty well.


I did that, changing it to

     while(Motor1.position(degrees) < target)

and it worked