Please Help with my PID template i found

ssierakowski · October 28, 2024, 6:52pm

How do i figure this out, i found the template and i want to figure out pid, can somebody please walk me through how the pid works

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Thu Sep 26 2019                                           */
/*    Description:  Competition Template                                      */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Drivetrain           drivetrain    9, 10, 1, 2, 5
// Controller1          controller
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <iostream>
#include <math.h>
#include <tgmath.h>

using namespace vex;
// A global instance of competition
competition Competition;

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

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

// void pid(int finalHeading) {
//   int directionScalar = 1;

//   if (finalHeading > 180) {
//     directionScalar = -1;
//   }

//   double intialPosition = Drivetrain.rotation(degrees);

//   // double inchesPerDegree = 12.56 / 360;

//   double degreesToTravel = Distance * inchesPerDegree;

//   double finalPosition = degreesToTravel + intialPosition;

//   double currentPosition;

//   double p;

//   double pScaler;

//   double percentRobo;

//   while (fabs(finalPosition - Drivetrain.rotation(degrees)) < 15) {
//     currentPosition = Drivetrain.rotation(degrees);

//     p = finalPosition - Drivetrain.rotation(degrees);

//     pScaler = 100.0;

//     percentRobo = p * pScaler;

//     if (fabs(percentRobo) < 10) {
//       if (percentRobo < 0) {
//         percentRobo = -10;
//       } else if (fabs(percentRobo) > 10) {
//         percentRobo = 10;
//       }
//     }

//     Drivetrain.setDriveVelocity(percentRobo, percent);

//     Drivetrain.drive(forward);

//     // if(fabs(finalPosition - Drivetrain.rotation(degrees)) < .2)   {
//     // std :: cout << finalPosition << "\n";
//     // std :: cout << fabs(finalPosition - Drivetrain.rotation(degrees)) <<
//     // "\n"; Drivetrain.stop(); break;
//     //}

//     // Controller1.Screen.clearScreen();
//     // Controller1.Screen.setCursor(0, 0);
//     // Controller1.Screen.print(Drivetrain.rotation(degrees));

//     std ::cout << Drivetrain.rotation(degrees) << "\n";
//   }
//   // std :: cout << finalPosition << "\n";
//   std ::cout << fabs(finalPosition - Drivetrain.rotation(degrees)) << "STOP
//   \n";
//   // std :: cout << Drivetrain.rotation(degrees) << " - " << finalPosition <<
//   // "STOP \n";
//   Drivetrain.stop();

//   // Needs:
//   // Somehow save the starting posn of the drivetrain
// }
typedef struct { // init a reusable list of variables
  float current;
  float kP;
  float kI;
  float kD;
  float target;
  float error;
  float integral;
  float derivative;
  float lastError;
  float threshold;
  float totalError;

} pid;

pid turnPID;
pid leftDrive;
pid rightDrive;

// Cookie-Cutter PID loop for intelligently reaching a desired destination.
float turningPID(float iDes, int deg, float kP, float kI, float kD,
                 float kILimit) {
  turnPID.current = deg;
  turnPID.error = iDes - turnPID.current;
  if (fabs(turnPID.error) > 180) {
    turnPID.error -= copysign(1.0, turnPID.error) * 360;
  }
  turnPID.integral += turnPID.error;
  if (kI != 0) // integral - if Ki is not 0
  {            // If we are inside controllable window then integrate the error
    if (fabs(turnPID.error) < kILimit)
      turnPID.integral = turnPID.integral + turnPID.error;
    else
      turnPID.integral = 0;
  } else // Otherwise set integral to 0
    turnPID.integral = 0;
  turnPID.derivative =
      turnPID.error - turnPID.lastError; // Calculate Derivative
  turnPID.lastError = turnPID.error;

  float thePower = (turnPID.error * kP) + (turnPID.integral * kI) +
                   (turnPID.derivative * kD);

  return (thePower);
}

void turnToHeadingPID(float iDes, float kP, float kI, float kD, float kILimit) {
  DrivetrainInertial.setHeading(0, degrees);
  // iDes = iDes % 360;
  iDes = fmod(iDes, 360);
  //  constrainAngle(iDes);
  float currHeading = DrivetrainInertial.heading();
  const float minPower = 2.7;
  // tune min power
  while (fabs(currHeading - iDes) > 0.1) {
    // tolerance
    currHeading = DrivetrainInertial.heading();
    float powerToDrive = turningPID(iDes, currHeading, kP, kI, kD, kILimit);

    if (fabs(powerToDrive) < minPower && powerToDrive != 0) {
      if (powerToDrive < 0) {
        powerToDrive = -minPower;
      } else {
        powerToDrive = minPower;
      }
    }

    LeftDriveSmart.spin(forward, powerToDrive, voltageUnits::volt);
    RightDriveSmart.spin(reverse, powerToDrive, voltageUnits::volt);
    wait(10, msec);
    if (powerToDrive == 0) {
      break;
    }
  }

  Drivetrain.stop();
}

float calcVoltDrive(float kP, float kI, float kD, float destination,
                    float currentPosition, float kiLimit, pid currentPID) {

  currentPID.current = currentPosition;

  currentPID.kP = kP;

  currentPID.kI = kI;

  currentPID.kD = kD;

  currentPID.target = destination;

  currentPID.threshold = kiLimit;

  currentPID.error = destination - currentPosition;
  // position = error
  currentPID.derivative = currentPosition - currentPID.lastError;

  currentPID.integral += currentPID.error;

  if (currentPID.integral > kiLimit) {
    currentPID.integral = 0;
  }

  float finalVoltage = currentPID.error * kP + currentPID.derivative * kD +
                       currentPID.integral * kI;

  currentPID.lastError = currentPID.error;

  return (finalVoltage);
}

void lateralMovement(float kP, float kI, float kD, float destination,
                     float kiLimit) {

  float degPerInch = 360 / 12.5;

  float degDestination = destination * degPerInch;

  LeftDriveSmart.setPosition(0, degrees);
  RightDriveSmart.setPosition(0, degrees);

  // calcVoltDrive(kP, kI, kD, degDestination, LeftDriveSmart.position(degrees),
  // kiLimit,leftDrive); calcVoltDrive(kP, kI, kD, degDestination,
  // RightDriveSmart.position(degrees), kiLimit,rightDrive);

  leftDrive.error = degDestination - LeftDriveSmart.position(degrees);
  rightDrive.error = degDestination - RightDriveSmart.position(degrees);

  while ((leftDrive.error + rightDrive.error) / 2 > 30) /*tune this number. was 201.59*/    {
    float leftVoltage =
        calcVoltDrive(kP, kI, kD, degDestination,
                      LeftDriveSmart.position(degrees), kiLimit, leftDrive);
    float rightVoltage =
        calcVoltDrive(kP, kI, kD, degDestination,
                      RightDriveSmart.position(degrees), kiLimit, rightDrive);

    // move code
    LeftDriveSmart.spin(forward, leftVoltage, voltageUnits ::volt);
    RightDriveSmart.spin(forward, rightVoltage, voltageUnits ::volt);

    float averageVolt = (leftVoltage + rightVoltage) / 2;
      /* testing this trying to break loop*/
      wait(10, msec);
      if (averageVolt < 1) {
        break;
      }
  }
  Drivetrain.stop();
  LeftDriveSmart.stop();
  RightDriveSmart.stop();

 
}

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


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

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

  // ..........................................................................
  // Insert autonomous user code here.
  // ..........................................................................
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
    // ........................................................................

    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);

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

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

edit: code tags added by mods, please remember to use them.

Max_Waldman · October 29, 2024, 4:40am

For learning how PID works in general, I highly recommend An Introduction to PID Controllers by George Gillard (download it from here).

If you want an explanation of the code you have, here is a brief one (assuming you have read the above document):

ssierakowski:

typedef struct { // init a reusable list of variables
  float current;
  float kP;
  float kI;
  float kD;
  float target;
  float error;
  float integral;
  float derivative;
  float lastError;
  float threshold;
  float totalError;

} pid;

This just is a placeholder for all of your PID-related variables.

These are different PID settings for turning and driving (I am not sure why leftDrive and rightDrive are separated; normally there is just a drivePID).

Turning PID function (iDes is desired angle in degrees, deg is current angle in degrees, and the rest are just constants)

Calculates error

This is just to get the turnPID error within the right range.

Accumulate the total error

I am not sure why the total error is accumulated twice, and I don’t think the previously mentioned line (turnPID.integral += turnPID.error;) should be there.

Set the previous error

Calculate power

That was just the PID; if you are confused about the other code I can help you. If you are having trouble with integral windup (explained in the George Gillard document), it might help to look at the part about integral windup in this post. You probably don’t really need it (you don’t really need the I term itself either), but it is nice to have.