How to debug my PID code?

VEX V5 General Discussion Technical Discussion
1

Hello everyone,

We decided to use two flying wheels to shoot the disks. I wrote the following PID function to adjust the motor speed so we could shoot continuously, but I really don’t know how to debug this this function using our V5 brain. I tried to print out some data using , but the data were all printed in a single line and chopped.

Would really appreciate some help here.

void fw_pid_rpm_with_time_limit(double lRpm, double rRpm, int time_limt_msec) {
double kp = 0.6;
double ki = 0.6;
double kd = 0.7;
int total_wait_time_msec = 0;

//==============================================================
// PID variables for left and right flying wheels.
//==============================================================
double error_l = 0;
double last_error_l = 0;
double error_sum_l = 0;
double d_l;

double error_r = 0;
double last_error_r = 0;
double error_sum_r = 0;
double d_r;
//==============================================================
// Set up the motor speed for both left and right flying wheels.
do {
double l_rpm = left_fw_motor.velocity(rpm);
double r_rpm = right_fw_motor.velocity(rpm);

  error_l = l_rpm - lRpm;
  error_r = r_rpm - rRpm;
  error_sum_l += error_l;
  error_sum_r += error_r;
  
  d_l = kp * error_l + ki * error_sum_l + kd * (error_l - last_error_l);
  d_r = kp * error_r + ki * error_sum_r + kd * (error_r - last_error_r);

  left_fw_motor.spin(fwd, lRpm - d_l, velocityUnits::rpm);
  right_fw_motor.spin(fwd, rRpm - d_r, velocityUnits::rpm);

  last_error_l = error_l;
  last_error_r = error_r;
  
  wait(10, msec);
  total_wait_time_msec += 10;
  Brain.Screen.print("L:%4.2f,R:%4.2f\n", l_rpm, r_rpm);

} while ((total_wait_time_msec < time_limt_msec) &&
fabs(error_l) < 5 &&
fabs(error_r) < 5);
Brain.Screen.print(“I:%d\n”, i);
}

2

If you want the values to keep updating instead of printing after each other, you need to clear the screen and reprint the values. This can be done by adding these lines inside your loop:

Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
1 Like
3

Thanks a lot, I guess my general question would be what’s the best way to debug my pid code written in c++.

Ideally, I would like to print a lot of information, since there are so many loops, there will be a lot of information accumulated during a test run, how can I dump all the information in my computer so I can analyze the output and debug?

Thanks

4

One method I used last year was dumping all the data into the SD card. I had a separate file for each variable and each line was a separate data point that also had the timestamp. I could then put the SD card into my computer and import all the data into excel which allowed me to graph it. Here are some resources if you need help with using the SD card:

1 Like
5

Thanks, I will try it out and report back.