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. */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Drivetrain drivetrain 1, 10
// RangeFinderC sonar C, D
// ---- END VEXCODE CONFIGURED DEVICES ----
#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()
{
while(1){
double SensorValue = RangeFinderC.distance( inches );
error = SensorValue - Distance;
speed = Kp * error + DerivativeValue + IntergalValue;
Derivative = NewError - error;
DerivativeValue = Derivative * Kd;
IntergalValue = Ki * Ierror;
task::sleep(10);
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","derVAL:",DerivativeValue);
Brain1.Screen.print("%.2f","interVAL:", IntergalValue);
wait(200, msec);
Brain1.Screen.setCursor(1, 1);
Brain1.Screen.clearScreen();
wait(5, msec);
task::sleep(5);
NewError = error;
Ierror += NewError;
}
}
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// 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.
}