This is my code for drive pid:
void driveTo(double distance_in, double time_limit_msec, bool exit, double max_output) {
// Store initial encoder values
double start_left = getLeftRotationDegree(), start_right = getRightRotationDegree();
stopChassis(vex::brakeType::coast);
is_turning = true;
double threshold = 0.5;
int drive_direction = distance_in > 0 ? 1 : -1;
double max_slew_fwd = drive_direction > 0 ? max_slew_accel_fwd : max_slew_decel_rev;
double max_slew_rev = drive_direction > 0 ? max_slew_decel_fwd : max_slew_accel_rev;
bool min_speed = false;
if(!exit) {
// Adjust slew rates and min speed for chaining
if(!dir_change_start && dir_change_end) {
max_slew_fwd = drive_direction > 0 ? 24 : max_slew_decel_rev;
max_slew_rev = drive_direction > 0 ? max_slew_decel_fwd : 24;
}
if(dir_change_start && !dir_change_end) {
max_slew_fwd = drive_direction > 0 ? max_slew_accel_fwd : 24;
max_slew_rev = drive_direction > 0 ? 24 : max_slew_accel_rev;
min_speed = true;
}
if(!dir_change_start && !dir_change_end) {
max_slew_fwd = 24;
max_slew_rev = 24;
min_speed = true;
}
}
This is my code for Turn PID:
void turnToAngle(double turn_angle, double time_limit_msec, bool exit, double max_output) {
// Prepare for turn
stopChassis(vex::brakeType::coast);
is_turning = true;
double threshold = 1;
PID pid = PID(turn_kp, turn_ki, turn_kd);
// Normalize and set PID target
turn_angle = normalizeTarget(turn_angle);
pid.setTarget(turn_angle);
pid.setIntegralMax(0);
pid.setIntegralRange(3);
pid.setSmallBigErrorTolerance(threshold, threshold * 3);
pid.setSmallBigErrorDuration(50, 250);
pid.setDerivativeTolerance(threshold * 4.5);
// Draw baseline for visualization
double draw_amplifier = 230 / fabs(turn_angle);
Brain.Screen.clearScreen(black);
Brain.Screen.setPenColor(green);
Brain.Screen.drawLine(0, fabs(turn_angle) * draw_amplifier, 600, fabs(turn_angle) * draw_amplifier);
Brain.Screen.setPenColor(red);
// PID loop for turning
double start_time = Brain.timer(msec);
double output;
double current_heading = getInertialHeading();
double previous_heading = 0;
int index = 1;
if(exit == false && correct_angle < turn_angle) {
// Turn right without stopping at end
while (getInertialHeading() < turn_angle && Brain.timer(msec) - start_time <= time_limit_msec) {
current_heading = getInertialHeading();
output = pid.update(current_heading); // PID update for heading
// Draw heading trace
Brain.Screen.drawLine(index * 3, fabs(previous_heading) * draw_amplifier, (index + 1) * 3, fabs(current_heading * draw_amplifier));
index++;
previous_heading = current_heading;
// Clamp output
if(output < min_output) output = min_output;
if(output > max_output) output = max_output;
else if(output < -max_output) output = -max_output;
driveChassis(output, -output);
wait(10, msec);
}
} else if(exit == false && correct_angle > turn_angle) {
// Turn left without stopping at end
while (getInertialHeading() > turn_angle && Brain.timer(msec) - start_time <= time_limit_msec) {
current_heading = getInertialHeading();
output = pid.update(current_heading);
Brain.Screen.drawLine(index * 3, fabs(previous_heading) * draw_amplifier, (index + 1) * 3, fabs(current_heading * draw_amplifier));
index++;
previous_heading = current_heading;
if(output < min_output) output = min_output;
if(output > max_output) output = max_output;
else if(output < -max_output) output = -max_output;
driveChassis(-output, output);
wait(10, msec);
}
} else {
// Standard PID turn
while (!pid.targetArrived() && Brain.timer(msec) - start_time <= time_limit_msec) {
current_heading = getInertialHeading();
output = pid.update(current_heading);
Brain.Screen.drawLine(index * 3, fabs(previous_heading) * draw_amplifier, (index + 1) * 3, fabs(current_heading * draw_amplifier));
index++;
previous_heading = current_heading;
if(output > max_output) output = max_output;
else if(output < -max_output) output = -max_output;
driveChassis(output, -output);
wait(10, msec);
}
}
if(exit) {
stopChassis(vex::hold);
}
correct_angle = turn_angle;
is_turning = false;
}
I found that even if I change kp, ki, kd for turn PID, it changes nothing in the robot’s turning. Robot keeps going back and forth, oscillating (even if the kp is low, ki and kd is 0..). However, I don’t think that the code is the problem since I downloaded from github. What do you guys think?
edit: code tags added by mods, please remember to use them.