PID running after reaches a point and how to change target value to inches?

Programming Support
1 
void drivePID(double target){
 
 double error=6; 
 double prevError = 0;  // position 12ms ago
 double derivative;    
 double integral = 0; 
 // get position of four motors

  LFDrive.setPosition(0, degrees);
  LBDrive.setPosition(0, degrees);
  RFDrive.setPosition(0, degrees);
  RBDrive.setPosition(0, degrees);
    
 while (error>=1 || error<=-1){ 
   
  double LFMotorPosition = LFDrive.position(degrees);
  double LBMotorPosition = LBDrive.position(degrees);
  double RFMotorPosition = RFDrive.position(degrees);
  double RBMotorPosition = RBDrive.position(degrees);
  double kP = 0.6, kI = 0.0, kD = 0.0;
  double accelRate = 0.0;
 // get average of the four motors (sensor value)
  double averagePosition = ((LFMotorPosition + LBMotorPosition) / 2 + (RFMotorPosition + RBMotorPosition) / 2);
 
 // position
  error = target - averagePosition;
  // derivative
  derivative = error - prevError;

  // integral 
  integral += error;

 double lateralMotorPower = (error * kP + derivative * kD + integral * kI);
 if (fabs(target)/target==1){
   if(target<=error){
     accelRate += 1000;
     lateralMotorPower = accelRate;
   }
   else if(fabs(target)/target==-1){
     if (target >= error){
       accelRate += -1000;
       lateralMotorPower = accelRate;
     }
   }
 }
 if (lateralMotorPower >= 430){
   lateralMotorPower=430;
 }
 if (lateralMotorPower<=-430){
   lateralMotorPower=-430;
 }
 if(error<=0){
    integral=0;
    }
    
 LFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
 LBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
 RFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
 RBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
  prevError = error;
  task::sleep(12);
  
 

 }
}
2

PID keeps running slowly after it reaches the target. Any ideas? Also, ideas on how to change the target value aka drivePID(//target value) to inches?

3

What my team does is we use degrees or you could use rotations and each rotation is equal to diameter of wheels 3.14 so for example a 4" wheel3.14 = 12.56" traveled per rotation. You could use that logic. Or you could try and make it like

LeftMotor.setPosition(0,rotations);

int inches;
int circumference;
inches = rotations/circumference

Hope this helps!

1 Like
4

I think I found a better way to convert it into inches.
This is what I added in my code for PID:

LeftMotor.setPosition(0,degrees);

//Autonomous Settings
#define _USE_MATH_DEFINES
#include
double inches;
inches/(M_PI*4.125) *360 = degrees;
double desiredValue = inches;//this is the amount of degrees that you desire – change if needed

desiredValue = 10;//this number is the number of inches the wheels will move

EDIT:: Use ‘double’ instead of int because pi is irrational. Also, include the library and define the ‘_USE_MATH_DEFINES’ like what I have above.

Obviously this isn’t my whole code but this is the converting it to inches part. If you want me to post my whole code so you can see where in the code everything is, just ask! :grin: This is what I have in my PID test code too.