126 lines
4.3 KiB
C++
126 lines
4.3 KiB
C++
/**
|
|
* @file include/lemlib/pid.hpp
|
|
* @author Lemlib Team
|
|
* @brief FAPID class header
|
|
* @version 0.4.5
|
|
* @date 2023-01-15
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
#pragma once
|
|
#include <string>
|
|
#include "pros/rtos.hpp"
|
|
|
|
namespace lemlib {
|
|
/**
|
|
* @brief Feedforward, Acceleration, Proportional, Integral, Derivative PID controller
|
|
*
|
|
* The controller does not loop on its own. It must be called in a loop.
|
|
* For example: while(!controller.settled) { controller.update(input, output); }
|
|
*
|
|
*/
|
|
class FAPID {
|
|
public:
|
|
/**
|
|
* @brief Construct a new FAPID
|
|
*
|
|
* @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
|
|
* @param kA acceleration gain, limits the change in output. Set 0 if disabled
|
|
* @param kP proportional gain, multiplied by error and added to output
|
|
* @param kI integral gain, multiplied by total error and added to output
|
|
* @param kD derivative gain, multiplied by change in error and added to output
|
|
* @param name name of the FAPID. Used for logging
|
|
*/
|
|
FAPID(float kF, float kA, float kP, float kI, float kD, std::string name);
|
|
/**
|
|
* @brief Set gains
|
|
*
|
|
* @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
|
|
* @param kA acceleration gain, limits the change in output. Set 0 if disabled
|
|
* @param kP proportional gain, multiplied by error and added to output
|
|
* @param kI integral gain, multiplied by total error and added to output
|
|
* @param kD derivative gain, multiplied by change in error and added to output
|
|
*/
|
|
void setGains(float kF, float kA, float kP, float kI, float kD);
|
|
/**
|
|
* @brief Set the exit conditions
|
|
*
|
|
* @param largeError range where error is considered large
|
|
* @param smallError range where error is considered small
|
|
* @param largeTime time in milliseconds t
|
|
* @param smallTime
|
|
* @param maxTime
|
|
*/
|
|
void setExit(float largeError, float smallError, int largeTime, int smallTime, int maxTime);
|
|
/**
|
|
* @brief Update the FAPID
|
|
*
|
|
* @param target the target value
|
|
* @param position the current value
|
|
* @param log whether to check the most recent terminal input for user input. Default is false because logging
|
|
* multiple PIDs could slow down the program.
|
|
* @return float - output
|
|
*/
|
|
float update(float target, float position, bool log = false);
|
|
/**
|
|
* @brief Reset the FAPID
|
|
*/
|
|
void reset();
|
|
/**
|
|
* @brief Check if the FAPID has settled
|
|
*
|
|
* If the exit conditions have not been set, this function will always return false
|
|
*
|
|
* @return true - the FAPID has settled
|
|
* @return false - the FAPID has not settled
|
|
*/
|
|
bool settled();
|
|
/**
|
|
* @brief initialize the FAPID logging system
|
|
*
|
|
* if this function is called, std::cin will be used to interact with the FAPID
|
|
*
|
|
* the user can interact with the FAPID through the terminal
|
|
* the user can access gains and other variables with the following format:
|
|
* <name>.<variable> to get the value of the variable
|
|
* <name>.<variable>_<value> to set the value of the variable
|
|
* for example:
|
|
* pid.kP_0.5 will set the kP value to 0.5
|
|
* list of variables thats value can be set:
|
|
* kF, kA, kP, kI, kD
|
|
* list of variables that can be accessed:
|
|
* kF, kA, kP, kI, kD, totalError
|
|
* list of functions that can be called:
|
|
* reset()
|
|
*/
|
|
static void init();
|
|
private:
|
|
float kF;
|
|
float kA;
|
|
float kP;
|
|
float kI;
|
|
float kD;
|
|
|
|
float largeError;
|
|
float smallError;
|
|
int largeTime = 0;
|
|
int smallTime = 0;
|
|
int maxTime = -1; // -1 means no max time set, run forever
|
|
|
|
int largeTimeCounter = 0;
|
|
int smallTimeCounter = 0;
|
|
int startTime = 0;
|
|
|
|
float prevError = 0;
|
|
float totalError = 0;
|
|
float prevOutput = 0;
|
|
|
|
void log();
|
|
std::string name;
|
|
static std::string input;
|
|
static pros::Task* logTask;
|
|
static pros::Mutex logMutex;
|
|
};
|
|
} // namespace lemlib
|