Need PID help with integral and derivative

Hey, i am looking at my PID loop and am having difficulties with my integral and derivative. Is there any obvious issues with the code? Thanks!

/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Tue Oct 01 2019                                           */
/*    Description:  Detecting Distances                                       */
/*                  This program will use the Range Finder to stop a robot    */
/*                  300 mm away from a wall.                                  */
/*                                                                            */

// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Drivetrain           drivetrain    1, 10           
// RangeFinderC         sonar         C, D            

#include "vex.h"

using namespace vex;
motor RightDriveSmart = motor(PORT10);
motor LeftDriveSmart = motor(PORT1);

brain  Brain1;

double Distance = 15;
double SensorValue;
double error = 0;
float speed;

float Kp = 0.3;

float Kd = 0.1;
double Derivative;
double DerivativeValue;
double NewError; 

float Ki = .01;
double Ierror;
double IntergalValue; 

int pid()

  double SensorValue = RangeFinderC.distance( inches );

  error = SensorValue - Distance;

  speed = Kp * error + DerivativeValue + IntergalValue; 

  Derivative = NewError - error;

  DerivativeValue = Derivative * Kd;

  IntergalValue = Ki * Ierror;



  wait(200, msec);
 LeftDriveSmart.spin(reverse, speed, volt);
  RightDriveSmart.spin(fwd, speed, volt);
  Brain1.Screen.print("%.2f","dist:", RangeFinderC.distance(inches));
  Brain1.Screen.print("%.2f","err:", error);
  Brain1.Screen.print("%.2f","der:" ,Derivative);
  Brain1.Screen.print("%.2f","interVAL:", IntergalValue);

   wait(200, msec);
    Brain1.Screen.setCursor(1, 1);
    wait(5, msec);

NewError = error;
Ierror += NewError;


int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  // Wait 1 second before driving forward.
  wait(1, seconds);
vex::task sheesh (pid);
  // The robot will stop driving when the Range Finder is less than 300 mm away
  // from an object.

Fixed. 20 Characters

1 Like

``` before the first line and ``` after the last line. What difficulties are you having? This

is not very helpful.

The robot goes forward infinitely, no matter the distance (this loop works with an ultrasonic sensor). This happens even when the values are altered, and no matter the distance read from the sensor.

What results are you seeing in your display?

Without looking at your math, I am a little concerned with the number of wait you are using. 10+200+200+5+5 is 420 ms per cycle. That’s almost a half second each time before it adjusts the PID. You should also check and see what the distance sensor sample rate is.

You may want to split the display and the PID into separate tasks so they can each run at different rates. Let me know if you need more information on doing this.