Still having PID trouble

And I would just substitue the position finding for the encoder values right?

The first Q answer depends on if your drive slips and how good your tracking wheels keep contact with the ground. It appears to me that many teams believe this is the case. My observation is that the encoders in the V5 motors are vastly superior to the 393 add-on ones. They also don’t suffer from the bent or misaligned shaft issues that can happen on the optical encoders.

The second Q answer is yes. However, you need to note if there is a difference in units between the internal encoder and the external encoder. If the API support degrees for both then it is a simple switch out (for driving straight). However, if you can only get counts from the encoder then the gain values will need to be adjusted.

I’m pretty sure turn values (sent to the PID) will be different (to achieve the same turn) since the encoders for the motor and the external will be different distances from the center of rotation.

1 Like

Well, do you think it would be better to use an inertial sensor to handle the turning?

I don’t have any experience using the inertial sensor. However, it would appear there are many teams that rely on it. The chatter on this forum seems to be positive.

1 Like

Yes Thank you for all of your help!!!

One more question when I run the target value and make is say 200 and it does everything correctly, once it gets there my motors slowly move forward. Does this just mean I have to tune my kp kI and kD more?

//
// P - Proportional ( Like P-Control )
//
// I - Integral ( )
//
// D - Derivative ( )
//
//

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

extern inertial Inertial20;

extern motor LeftMotor;
extern motor RightMotor;
extern motor BLM;
extern motor BRM;

extern drivetrain Drivetrain;

extern controller Controller1;

extern inertial InertialS;

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

// Settings
double kP = 0.16; // Error
double kI = 0.000001; // Last, Increase Precision
double kD = 0.32; // To Fast or Too Slow

double turnkP = 0.0;
double turnkI = 0.00;
double turnkD = 0.0;

// Auton Settings
double targetValue = 200;
double targetTurnValue = 0;

double error;             // SensorValue - DesireValue : Positional Value
double previouserror = 0; // Position 20 miliseconds ago
double derivative;        // error - previouserror : Speed
double totalerror = 0;    // totalerror = totalerror + error

double turnerror;             // SensorValue - DesireValue : Positional Value
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative;        // error - previouserror : Speed
double turntotalerror = 0;    // totalerror = totalerror + error

bool resetDriveSensors = true;

// Variation modified for use
bool enableDrivePID = true;

bool pidAbort = false; // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value

int drivePID() {

  while(1) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
      totalerror = 0.0;
      previouserror = 0.0;
      turntotalerror = 0.0;
      turnpreviouserror = 0.0;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    } // resetDriveSensors

    
      if (enableDrivePID) {
       
       // Get Position of Both Motors
        double leftMotorPosition = LeftMotor.position(degrees);
        double rightMotorPosition = RightMotor.position(degrees);

        // Average Of the Motors
        double averagePosition = (leftMotorPosition + rightMotorPosition) / 2;

        // Proportional
        error = targetValue - averagePosition;

        // Derivative
        derivative = error - previouserror;

        // Integral : Velocity -> Position -> Absenent ( Position x Time )
        totalerror += error;

        double lateralMotorPower = (error * kP + derivative * kD + totalerror * kI);

        // Average Of the Motors
        double turnDifference = leftMotorPosition - rightMotorPosition;

        // Proportional
        turnerror = turnDifference - targetTurnValue;

        // Derivative
        turnderivative = turnerror - turnpreviouserror;

        // Integral : Velocity -> Position -> Absenent ( Position x Time )
        turntotalerror += turnerror;

        double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);

        ///////////////////////////////////////////////////////////////////////

        LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower, voltageUnits::volt);
        RightMotor.spin(forward, lateralMotorPower + turnMotorPower,  voltageUnits::volt);

        previouserror = error;
        turnpreviouserror = turnerror;

        // stop PID from controlling motor when at target  }

        if ((fabs(error) < deadBand) || pidAbort) {

        enableDrivePID = false;

      } // enableDrivePID

    task::sleep(20);
     
  } // While (1)
  }

  return 1;

}

void pidMoveToPosition(double setPoint, double setTurn, bool wait4done = true) {
  resetDriveSensors = true;
  targetValue = setPoint;
  targetTurnValue = setTurn;
  enableDrivePID = true;
  while (enableDrivePID & wait4done) {
    pidAbort = Controller1.ButtonB.pressing(); // abort PID by press button B
    vex::task::sleep(20);
  }
  pidAbort = false;
  return;
}

////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
// This is Inertial Sensor PID Part




////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////

/*---------------------------------------------------------------------------*/

void autonomous(void) {

  vex::task start(drivePID);
  targetValue = 200;

  task::sleep(6000);

  targetValue = 0;

  /*Inertial20.calibrate(false);

  while (Inertial20.isCalibrating()) {
    wait(1, msec);
  }

  LeftMotor.spin(directionType::fwd);
  RightMotor.spin(directionType::fwd);
  BLM.spin(directionType::fwd);
  BRM.spin(directionType::fwd);

  waitUntil(Inertial20.rotation(degrees) >= 270);

  LeftMotor.stop();
  RightMotor.stop();
  BLM.stop();
  BRM.stop();*/ // Interial Turnging Code
}

void usercontrol(void) {

  enableDrivePID = false;



    // ADD THIS CODE - it doesn't mater if it is before or after Driver Control
    // code start PID by pressing button A
    if (Controller1.ButtonA.pressing()) {
      pidMoveToPosition(720, 0, true);
      pidMoveToPosition(-720, 0, true);

      wait(20, msec); // Sleep the task for a short amount of time to
                      // prevent wasted resources.
    }
  
}

  //
  // Main will set up the competition functions and callbacks.
  //
  int main() {
    // Set up callbacks for autonomous and driver control periods.
    Competition.autonomous(autonomous);
    Competition.drivercontrol(usercontrol);

    vex::task t1(drivePID);

    // Run the pre-autonomous function.
    pre_auton();

    // Prevent main from exiting with an infinite loop.
    while (true) {
      wait(100, msec);
    }
  }

Probably, the value for kD seems high. You can tell by printing their values to the screen. Create a variable for each term then you can see what they are contributing to the drive power near the end of the path.

      Brain.Screen.printAt(10, 30, "pTerm  %2.3f", pTerm);
      Brain.Screen.printAt(10, 60, "iTerm  %2.3f", iTerm);
      Brain.Screen.printAt(10, 90, "dTerm  %2.3f", dTerm);
      Brain.Screen.printAt(10, 120, "curPos %5.2f", currentPosition);
2 Likes

The error is equal to almost 140 at the beggining and moving at max speed. The Deriviative switches between 300 and 0. Finally the Integral ends with a total error of around 142.
When I did this liitle bit of code in auton the wheels just keep moving after it reaches a certain point.

void autonomous(void) {

  vex::task start(drivePID);

  targetValue = 200;

  task::sleep(7500);

  targetValue = 0; 
}

Right, since we are not using hold power, we (that is I) forgot that when exiting the PID the motors need to be set to 0 so any built up iTerm or residual error in the pTerm is not left as the last command to motors.spin(). If you made deadBand=0, then eventually the PID output would be 0 as long as there are no forces working against the drive motors like a hill.

so just set the motors to 0 in the if ((fabs(error)<deadBand) || pidAbort) { block.

ed: adding @JD6 b/c i forgot this in that thread also.

1 Like

Thanks for tagging me. I had noticed that issue, and I just added that myself.

1 Like