Lift Pid running in drivetrain

VEX Robotics Competition Discussion
1 
#include "vex.h"

using namespace vex;

competition Competition;

//remeber to set Rotation to 0 before actually turning anything

int x = 0;

int osc = 0;

int tod = 1;

int skil =1;

int hang = 2;

int oscState = 0; // 0: Not oscillating, 1: Oscillating

bool liftPIDRunning = false;

double target = 278;

int direction = -1;

int liftpid(){
liftPIDRunning = true;
 Lift.setStopping(hold);
  double kp = 0.018; // Proportional gain (adjust as needed)
  double ki = 0.003; // Integral gain (adjust as needed)
  double kd = 0.02;  // Derivative gain (if needed)
  double min_speed = 100; // Minimum speed to maintain movement
  double max_speed = 100; // Maximum speed limit
  double current = Rotation8.position(deg); // Current position

  Rotation8.resetPosition(); // Reset encoder position

  double integral = 0.0; // Initialize integral term
  double previous_error = target - current; // Initialize previous error

  while (liftPIDRunning && fabs(target - current) > 1) {
    current = Rotation8.position(deg); // Read current position
    double error = target - current; // Calculate error

    // Update integral term with anti-windup
    if (fabs(integral) < max_speed / ki) {
      integral += error;
    }

    // Calculate speed based on PID control
    double speed = kp * error + ki * integral + kd * (error - previous_error);

    // Update previous error for derivative term
    previous_error = error;

    // Ensure speed is within limits
    if (speed > max_speed) {
      speed = max_speed;
    } else if (speed < -max_speed) {
      speed = -max_speed;
    } else if (fabs(speed) < min_speed) {
      speed = min_speed * direction; // Maintain minimum speed for smooth movement
    }

    // Apply speed to the motor
    Lift.spin(direction > 0 ? forward : reverse, fabs(speed), percent);
    Lift2.spin(direction > 0 ? forward : reverse, fabs(speed), percent);
    task::sleep(20); // Delay for stability
  }

  Lift.stop(hold); // Stop the motor when target is reached
  Lift2.stop(hold);
  liftPIDRunning = false;
  return 1;
}
void driverControl(){
 liftPIDRunning = false;
while(true){
LeftBottom.spin(vex::directionType::fwd, (Controller1.Axis1.value() + Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
LeftBack.spin(vex::directionType::fwd, (Controller1.Axis1.value() + Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
LeftFront.spin(vex::directionType::fwd, (Controller1.Axis1.value() + Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
RightBottom.spin(vex::directionType::fwd, (Controller1.Axis1.value() - Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
RightFront.spin(vex::directionType::fwd, (Controller1.Axis1.value() - Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
RightBack.spin(vex::directionType::fwd, (Controller1.Axis1.value() - Controller1.Axis3.value()*1.8), vex::velocityUnits::pct);
Lift.setStopping(hold);
Lift2.setStopping(hold);
ClawRotate.setStopping(hold);


if (hang==1 ){
    Hang.set(true);
}
else if (hang==2 ){
     Hang.set(false);
}
if (Controller1.ButtonA.pressing() && hang == 2){

hang = 1;
vex::task::sleep(150);
}

if (Controller1.ButtonA.pressing() && hang == 1){
  hang = 2;
  vex::task::sleep(150);

}
if(Controller1.ButtonL1.pressing()){
Lift.spin(forward,100, percent);
Lift2.spin(forward,100, percent);
}

else if(Controller1.ButtonL2.pressing()){
Lift.spin(reverse,100, percent);
Lift2.spin(reverse,100, percent);
}

else{
Lift.stop();
Lift2.stop();
}

if(Controller1.ButtonR1.pressing()){
ClawRotate.spin(forward,100, percent);
}
else{
ClawRotate.stop();
}

if(Controller1.ButtonRight.pressing()){ //neutral mode
Circuit1.set(false);
//Circuit2.set(false);
}
 if(Controller1.ButtonUp.pressing()){ //open mode
Circuit1.set(true);
//Circuit2.set(true);
}
 if(Controller1.ButtonDown.pressing()){ //closed mode
Circuit1.set(true);
//Circuit2.set(false);
x = 1;
}
if (tod==1 ){
    Mogo.set(true);
}
else if (tod==2 ){
     Mogo.set(false);
}
if (Controller1.ButtonX.pressing() && tod == 2){
tod = 1;
vex::task::sleep(150);
}

if (Controller1.ButtonX.pressing() && tod == 1){
  tod = 2;
  vex::task::sleep(150);

}
    if (Controller1.ButtonR2.pressing() && !liftPIDRunning) {
      // Start liftpid task only if it's not already running
      liftPIDRunning = true;
      target = 278; // Set your desired target
      direction = -1; // Set direction based on your setup
      vex::task willy_chews(liftpid); // Start the PID task
    }
    wait (20, msec);
}
}
void Autonomous() {
  
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

Competition.autonomous(Autonomous);
Competition.drivercontrol(driverControl);
}

I’m trying to run my lift PID on Button R2 and it is currently running but its movement is wild and it seems to be moving in intervals, it also completely misses its target of 278. I tested this same pid with the same values inside autonomous and it worked flawlessly. Any advice?

edit: code tags added by moderators, please remember to use them.

2

This will still be running. Don’t control motors in more than one place.

2 Likes
3

That worked perfectly thank you. Do you have any ideas on how I could stop the motors from spinning after L1 or L2 is released

4

perhaps something like this.

if(Controller1.ButtonL1.pressing()){
  liftPIDRunning = false; // may need small delay after this
  Lift.spin(forward,100, percent);
  Lift2.spin(forward,100, percent);
}

else if(Controller1.ButtonL2.pressing()){
  liftPIDRunning = false; // may need small delay after this
  Lift.spin(reverse,100, percent);
  Lift2.spin(reverse,100, percent);
}

else{
  if( !liftPIDRunning ) {
    Lift.stop();
    Lift2.stop();
  }
}
2 Likes
5

I would also suggest turning your lift into a single motor group, and when making a PID, start with just P, then add D, then I.