Pid Speed Problem

Im using odometry and the odometry works but im having trouble with my pid going really slow it goes to correct point but it is really slow

// Drive Pd Settings
double kP =2.3;
double kD = 0.45;
double kRC = 1;
double kLC = 1;
void DriveToPointPid(double desiredX,double desiredY){
TurntoPoint(desiredX,desiredY);
double distance = calculatedistance(xPos,desiredX, yPos,desiredY);
int startingLeftValue = LeftEncoderY.position(deg);
int startingRightValue = RightEncoderY.position(deg);
bool enablePD = true;
double Lerror = 0;
double LprevError = 0;
double Lderivative = 0;
double Rerror = 0;
double RprevError = 0;
double Rderivative = 0;
if (distance < 0){
  distance =  distance * -1;
}
while (enablePD){
double Labsoluteposition = (LeftEncoderY.position(deg) - startingLeftValue)*0.02396644697*0.99009901*kLC;
Lerror = Labsoluteposition - distance;

Lerror = Lerror * -1;
Lderivative = Lerror - LprevError;
double LmotorPower = (Lerror *kP + Lderivative*kD);
double Rabsoluteposition =( RightEncoderY.position(deg) - startingRightValue) * 0.02396644697*0.99009901*kRC;
Rerror = Rabsoluteposition - distance;
Rerror = Rerror * -1;
Rderivative = Rerror - RprevError;
double RmotorPower = (Rerror * kP + Rderivative * kD);
LmotorPower = keeplessthan(LmotorPower, LmotorPower, 3);
RmotorPower = keeplessthan(RmotorPower, RmotorPower, 3);
Brain.Screen.setCursor(7, 1);
Brain.Screen.print(distance);
if (distance < 0) {
    if (Labsoluteposition < distance) {
        LmotorPower = 0;
    }
}
else {
    if (Labsoluteposition > distance) {
        LmotorPower = 0;
    }
}
if (distance < 0) {
    if (Rabsoluteposition < distance) {
        RmotorPower = 0;
    }
}
else {
    if (Rabsoluteposition > distance) {
        RmotorPower = 0;
    }
}
if (distance < 0) {
    if ((Rabsoluteposition < distance) && (Labsoluteposition < distance)) {
        enablePD = false;
    }
}
else {
    if (Rabsoluteposition > distance && (Labsoluteposition > distance)) {
        enablePD = false;
    }
}
LF.spin(forward,LmotorPower,percent);
LR.spin(forward,LmotorPower,percent);
RF.spin(forward,RmotorPower,percent);
RR.spin(forward,RmotorPower,percent);
LprevError = Lerror;RprevError = Rerror;
task::sleep(20);
}
stopallmotors(brake);
}```

Would these lines:

LmotorPower = keeplessthan(LmotorPower, LmotorPower, 3);
RmotorPower = keeplessthan(RmotorPower, RmotorPower, 3);

cause the percent to be a max of 3? That means that the maximum speed of the motors is 3%. Probably change the 3 to something closer to 100.

Personally, I multiply the output of the PID controller (I usually keep it between -1~1) by the maximum rpm I want it to be.

I do not know about odom, but I have done a PID before. Is your issue that it goes too slow the whole way, or just the end. If it is just at the end you might want to look at this from George Gillard’s PID guide.


if you want the whole document you can find it here: http://georgegillard.com/documents/2-introduction-to-pid-controllers

1 Like

Im using odmetry for the distances
like show here

double distance = calculatedistance(XPos,x1, YPos, y1);

function:

double calculatedistance(double x0,double x1,double y0,double y1){
double d = sqrt(
 ( (x1-x0)*(x1-x0)) + ((y1-y0)*(y1-y0))
);
return d;
}

that is the only thing i use it for in the pd. The pd seems to go slow with short distances not long ones, so i would decrease the kp for short distances only?

that keeps it above three
to avoid osolation and to make it not stop at 0.1 power but 3
keeplessthan code:

double keeplessthan(double double1,double double2,double double3){
double product;
if (double2 < double1){
  product = double2;
  
}
else{
  if(double3 > double1){
    product = double3;
  }
  else{
  product = double1;
  }
}

Yes you may need to tune for different Kp and Kd values for different lengths and speeds.

The function name is kind of misleading but it should work fine as far as I can tell, considering you return product in this function.

Ok thank you i will try to tune it,with the kp and kd

I ended up having to completely reengineer my functions to better work for turning and driving thank you for the help.