What's wrong with my PID code?

Programming Support VEXcode Pro V5 Text Tech Support
#1

I want the robot to rotate 90 degrees, but it only rotates 25 degrees and stops

// trunRight
double KP = 0.2;
double KI = 0.0;
double KD = 0.0;
int error;                        //SensorValue - DesiredValue : Position
int prevError = 0;       //Position 20 miliseconds ago
int derivative;              // error - prevError : Speed
int totalError = 0;       //totalError = totalError + error

 void driver_tr(int target){
 Inertial15.setRotation(0,degrees);
 wait(20,msec);
 while(Inertial15.rotation(degrees) < target){
     int TurnValue = Inertial15.rotation(degrees);
     error = TrunValue - target;
     derivative = error - prevError;
     totalError += error; 
     double lateralMotorPower = error * KP + derivative * KD + totalError * KI;
     R1.spin(reverse,lateralMotorPower,voltageUnits::volt);
     R2.spin(reverse,lateralMotorPower,voltageUnits::volt);
     L1.spin(reverse,lateralMotorPower,voltageUnits::volt);
     L2.spin(reverse,lateralMotorPower,voltageUnits::volt);
     prevError = error;
     vex::task::sleep(20);
     }
  }
1 Like
#2

What does the inertial sensor read before and after you call driver_tr(90);?

One suggestion for debugging your function would be to have it output (to the screen, the controller, or the terminal) the Inertial sensor’s value and the lateralMotorPower.

1 Like
#3

Now there is a new situation, the robot starts to turn right, the value of inertial sensor increases from 0 to 45 and stops, but the machine continues to turn right

#4

The issue is with your exit condition: Once the inertial reading reaches the target value, it will exit the loop as the condition “inertial rotation >= target” is met. When this happens, the motor power will be set to whatever voltage the loop sets on the last iteration instead of zero, causing it to continue turning even though the loop have ended. The issue can be easily solved just by adding codes to set the motor power to 0 after the loop have exited.

However, there are also some other issues within your PID loop. First, I’ll strongly suggest changing all the pid variables (error, derivative etc.) into doubles as according to the vexcode API, .rotation() returns a double, which allows the loop to work with a wider range of values. Also, since you declared the PID variables to be global, be sure to initialize them to 0 at the beginning of each function so variables like integral will not infinitely increase every time you call turn. Personally however, I will simply declare the variables locally including the constants like the following, as there isn’t a purpose for the variables to be declared globally, and will only increase the risk of naming collisions

void driver_tr(double target){
    double kP = 0.2, kI = 0, kD = 0; // pid constants
    double error = 69, derivative = 0, integral = 0, prevError = 0;
    Inertial115.setRotation(0, degrees);
    // the rest of the code
}

Another problem with your pid loop is with the exiting condition. One of the most important feature of a PID loop is its ability to correct itself even after overshooting. This is possible since the output reverses as the error changes sign. However in your loop, once the turn overshoots, the control loop will instantly end and it’ll have no chance to autocorrect. To fix this, simply change the condition to:

std::fabs(error) < 0.1

This will allow the loop to continue to run permanently, until the error is small enough, meaning you are close enough to the target. Note that your error will need to be initialized to something that is larger than the deadband, or else the condition will be evaluated to true the moment you enter the loop an instantly exits. Alternatively, you can also use a do while loop to ensure that at least one iteration is done before the condition is checked.

I’ll also suggest writing a setPower() function which automatically sets the drive motor power based on the input. It will probably look something like this: (you’ll have to modify it since I don’t think vexcode can take in negative number of spin)

void setPower(double left, double right){
     R1.spin(forward, right, voltageUnits::volt);
     R2.spin(forward, right, voltageUnits::volt);
     L1.spin(forward, left, voltageUnits::volt);
     L2.spin(forward, left, voltageUnits::volt);
}

This will make the code a lot less repetitive since you can replace the four line you use to set motor power with just one line.

Also, just a slight question about the code: if you’re trying to turn, why is the motor power all set to the same value? Wouldn’t that make the robot drive forward instead of turn?

4 Likes
#5

His motors are probably reversed.

1 Like
#6

This is an excellent and underapprciated point. Writing code in ways that avoids repeating blocks is incredibly powerful. As written, the OP’s code will only work as a PID for turning in one direction. I would challenge roboteers on the forum to post code to this thread that would generalize to allowing the user to:

  • Turn right X degrees
  • Turn left X degrees
  • Go straight forward for distance
  • Go straight backward for distance
2 Likes
#7

challenge accepted

/**
 * @author Ryan Liao
 * @brief General Chassis PID Control
 * @date 2021-06-30
 *
 */

#include "okapi/api.hpp"
using namespace okapi;
std::shared_ptr<MotorGroup> left = std::make_shared<MotorGroup>(std::initializer_list<Motor>{1, 2}); // replace with your left motors' ports
std::shared_ptr<MotorGroup> right = std::make_shared<MotorGroup>(std::initializer_list<Motor>{3, 4}); // replace with your right motors' ports
std::shared_ptr<IMU> imu = std::make_shared<IMU>(5); // replace with inertial port

/**
 * @brief limits angle to range [-180, 180]
 *
 * @param angle raw angle
 * @return The Constrained Angle
 */
double constrainAngle(const double& angle){
    double res = std::fmod(angle + 180,360);
    if (res < 0)
        res += 360;
    return res - 180;
}

/**
 * @brief sets voltage for drive motor
 * 
 * @param lPower target left voltage
 * @param rPower target right voltage
 */
void setPower(const double& lPower, const double& rPower){
    left->moveVoltage(lPower);
    right->moveVoltage(rPower);
}

/**
 * @brief Averages the motor's IME reading
 *
 * @return Averaged encoder reading
 */
double getDriveEncoder(){
    return (left->getPosition() + right->getPosition()) / 2;
}

/**
 * @brief computes target power given pid variables
 *
 * @param kP kP
 * @param kI kI
 * @param kD kD
 * @param error error
 * @param derivative derivative
 * @param integral integral
 * @param prevError previous error
 * @return target power
 */
double pidCompute(const double& kP, const double& kI, const double& kD, double& error, double& derivative, double& integral, double& prevError){
    derivative = error - prevError;
    integral += error;
    prevError = error;
    return kP * error + kI * integral + kD * derivative;
}

/**
 * @brief Moves the robot for a certain encoder tick. Forward if target > 0, backward if target < 0
 *
 * @param target Target encoder tick to drive
 */
void moveDistance(const double& target){
    const double kP = 0, kI = 0, kD = 0; // replace with desired PID constasnt
    double error = 0, derivative = 0, integral = 0, prevError = 0, power = 0;

    do{
        error = target - getDriveEncoder();
        power = std::clamp(pidCompute(kP, kI, kD, error, derivative, integral, prevError), -12000, 12000);
        setPower(power, power);
    }while(std::fabs(error) > 0.1);
    setPower(0, 0);
}

/**
 * @brief Turns the robot for a certain angle. CW if target > 0, CCW if target < 0
 *
 * @param target Target angle to turn
 */
void turnAngle(const double& target){
    const double kP = 0, kI = 0, kD = 0; // replace with desired PID constant
    double error = 0, derivative = 0, integral = 0, prevError = 0, power = 0;
    // if you want target's unit to be inches, add a line of code to convert inches to ticks
    do{
        error = constrainAngle(target - imu->get());
        power = std::clamp(pidCompute(kP, kI, kD, error, derivative, integral, prevError), -12000, 12000);
        setPower(power, -power);
    }while(std::fabs(error) > 0.1);
    setPower(0, 0);
}

Note that I wrote this code in a fairly specific way by moving general PID calculations into a function (in this case pidCompute(). This, as said above, avoids needing to copy PID calculations everywhere whenever you need PID somewhere, making the code more precise. However, there is actually an even better way to implement this than using a function and passing every single required parameter. If anyone is interested, consider reading this article for more information.

4 Likes
#8

Thank you for your advice, which has helped me a lot. At present, I have achieved the robot rotation of 90 degrees, and the error is less than 0.5

#9

Wow , You guessed it

#10

Well done! This is the sort of coding approach that teams should take - functions that have a single and clear purpose that can be re-used.

2 Likes