Odometry and PID Half Working

[Max_Waldman]

Hi, I have a few suggestions for your code.

@Luke07

PID_Data::error = (targetX - odom.xPosGlobal) + (targetY - odom.yPosGlobal);

I know that you said that distance didn’t work well, but I think* that you should find the perpendicular distance to the point, or whatever it’s called (read the post in the *).

*

I have coded odom and use that method of finding distance to target (from 7842F 2018-19 Robot Showcase - Flywheel Control, Odometry, Engineering Journals & More) which in theory should work better, but I haven’t tested my odom yet anyway (we haven’t finished building our robot yet) so I might be wrong.

@Luke07

//only use integral if close enough to target but allows for tolerance
  if(fabs(PID_Data::error) < integralBound && fabs(PID_Data::error) > maxError) {
    PID_Data::integral += PID_Data::error; // Add the error to the integral
  } else PID_Data::integral = 0; // Reset the integral if we are too far from the target

(This applies to the turn integral too)
Even though you address integral windup by only letting the integral take effect when the error is in a certain range, you should still reset the total error (integral variable in your code) when the error crosses 0 (goes from positive to negative or vice versa), as this also helps with integral windup**.

**

According to Wikepedia (Integral windup - Wikipedia) that is called a Clegg integrator, but the article linked doesn’t even exist, so I am not sure. It seems to be mentioned in some research articles though, so that’s probably what it’s called.

@Luke07

leftFront.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
      leftMid.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
      leftBack.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
      rightBack.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);
      rightMid.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);
      rightFront.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);

I would use motor groups for this.

I think that the distance calculation is likely the main reason your code is failing, but I would also test it with both kI’s set to 0 to see if it is integral windup.