VEXCode PID Loop Issues

Programming Support VEXcode Pro V5 Text Tech Support
, , , ,
1

So I’m encountering an issue with a PID loop in my code where the loop is not being entered, or the task not being started at all. Not sure what the issue could be?

Related code: All in main.cpp.


double kp = 1.0;

double kd = 1.0;

int dist = 0;

int turnDeg = 0;

int error;

int prevError = 0;

int deriv;

int turnError;

int turnPrevError = 0;

int turnDeriv;

bool resetDrive = false;

bool enabledrivePD = true;

int drivePD() {
  Controller1.rumble("--");
  while (true) {

    if(enabledrivePD) {
      if (resetDrive) {
        resetDrive = false;
        LeftFrontDrive.setRotation(0, degrees);
        LeftMidDrive.setRotation(0, degrees);
        LeftRearDrive.setRotation(0, degrees);
        RightFrontDrive.setRotation(0, degrees);
        RightMidDrive.setRotation(0, degrees);
        RightRearDrive.setRotation(0, degrees);
      }
      int LeftRearPosition = LeftRearDrive.position(degrees);
      int LeftMidPosition = LeftMidDrive.position(degrees);
      int LeftFrontPosition = LeftFrontDrive.position(degrees);

      int RightRearPosition = RightRearDrive.position(degrees);
      int RightMidPosition = RightMidDrive.position(degrees);
      int RightFrontPosition = RightFrontDrive.position(degrees);
      double averagePosition =
          (RightFrontPosition + RightMidPosition + RightRearPosition +
          LeftRearPosition + LeftFrontPosition + LeftMidPosition) /
          6;
      Brain.Screen.print("Error: %d\n", error);
      Brain.Screen.print("Target: %d\n", dist);
      error = averagePosition - dist;

      deriv = prevError - error;

      double motorSpeed = kp * error + deriv * kd;

      turnError = averagePosition - turnDeg;
      turnDeriv = turnError - turnPrevError;
      //double motorturnSpeed = kp * turnError + turnDeriv *kd;
        
      LeftDrive.spin(forward, motorSpeed, voltageUnits::volt);

      RightDrive.spin(forward, motorSpeed,voltageUnits::volt);
    }
    wait(20, msec);
  }

  return 1;
}
void autonomous(void) {

task driveTask(drivePD);

dist = 2000;

vex::task::sleep(1000);

//Right Auton WP

/*Arms.spin(reverse);

wait(20,msec);

turnRight(20, 15);

driveForDist(10, 300);

FrontClaw.set(true);

driveForDist(20, -400);

turnLeft(40,20);*/

//Right Neutral Goal

// Arms.spin(reverse);

// driveForDist(80, 800);

// driveForDist(50, 50);

// driveForDist(20, 30);

// FrontClaw.set(true);

// driveForDist(80, -800);

//Left Auton WP

/*Arms.spin(reverse);

driveForDist(30, 100);

FrontClaw.set(true);

driveForDist(30, -200);

turnLeft(20,50);*/

}
2

I would recommend adding a controller rumble or something at various places in the while loop, I. e. right after your while (true), inside your first if, etc. This helps you figure out exactly what isn’t rrunning

The only possible issue I notice right off with your code is when you can the task, you say task driveTask(drivePD), while I usually say vex:task or maybe vex::task driveTask(drivePD). You could try it? I don’t really know the difference.

1 Like
3

As long as you are using namespace vex; somewhere in your code, it shouldn’t matter whether you use task or vex::task.

Other than that, this is a good troubleshooting step. If you have some sort of indicator as to where the code is when it stops running, then that could help you determine a course of action to take as you know where exactly the code stops.

I notice you have a rumble at the start of your drivePD function. Does this rumble work? This could be a good indicator as to where the problem is: if it rumbles, then we at least know that the task is running correctly. If it doesn’t, we know the task won’t run. Either way, it helps us diagnose the problem.

2 Likes
4

The Task isn’t actually running and I’m not sure why. The rumble there is meant to indicate that the task is starting but it’s not firing at all.

5

Our drivePD is a void program, not an int so not sure if I understand how it works exactly, but possibly - your dist variable is still 0 when drivePD is called. Try putting dist=2000 first?

6

So, I would do the PID toggle variable in the while loop instead of the if statement. So instead of while(true) { if (enabledrivePID){ //PID code } }
I would do

while (enabledrivePID){
//PID Code
}

Idk if that is the problem, but every PID I have seen does it this way, so there may be a reason for that.

1 Like
7

I found one thing in your code: error is supposed to be target - actual not actual - target which is what you get with

Can you put beeps or rumbles or lights or something in these places and tell us what happens:

Also what is the result of these printings:

2 Likes
8

The task is a while loop, so it should work such that

  1. The PD task starts running.
  2. The PD task enters the while loop.
  3. Meanwhile, the regular code continues running and dist = 2000;
  4. The PD task continues running the while loop, which realizes that error is large and translates that into motor motion.

Anyhow, if you have to set dist before you call the forever loop, you’ll never be able to change it and you’ll only be able to do one command.

2 Likes