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);

}