1*de74542cSJosh Lehan #pragma once 2*de74542cSJosh Lehan 3*de74542cSJosh Lehan #include "pid.hpp" 4*de74542cSJosh Lehan 5*de74542cSJosh Lehan #include <chrono> 6*de74542cSJosh Lehan #include <cstring> 7*de74542cSJosh Lehan #include <fstream> 8*de74542cSJosh Lehan #include <iostream> 9*de74542cSJosh Lehan #include <string> 10*de74542cSJosh Lehan 11*de74542cSJosh Lehan namespace pid_control 12*de74542cSJosh Lehan { 13*de74542cSJosh Lehan namespace ec 14*de74542cSJosh Lehan { 15*de74542cSJosh Lehan 16*de74542cSJosh Lehan // Trivial class for information exported from core PID loop function 17*de74542cSJosh Lehan struct PidCoreContext 18*de74542cSJosh Lehan { 19*de74542cSJosh Lehan double input; 20*de74542cSJosh Lehan double setpoint; 21*de74542cSJosh Lehan double error; 22*de74542cSJosh Lehan 23*de74542cSJosh Lehan double proportionalTerm; 24*de74542cSJosh Lehan double integralTerm1; 25*de74542cSJosh Lehan double integralTerm2; 26*de74542cSJosh Lehan 27*de74542cSJosh Lehan double derivativeTerm; 28*de74542cSJosh Lehan 29*de74542cSJosh Lehan double feedFwdTerm; 30*de74542cSJosh Lehan double output1; 31*de74542cSJosh Lehan double output2; 32*de74542cSJosh Lehan 33*de74542cSJosh Lehan double minOut; 34*de74542cSJosh Lehan double maxOut; 35*de74542cSJosh Lehan 36*de74542cSJosh Lehan double integralTerm3; 37*de74542cSJosh Lehan double output3; 38*de74542cSJosh Lehan 39*de74542cSJosh Lehan double integralTerm; 40*de74542cSJosh Lehan double output; 41*de74542cSJosh Lehan 42*de74542cSJosh Lehan bool operator!=(const PidCoreContext& rhs) const = default; 43*de74542cSJosh Lehan bool operator==(const PidCoreContext& rhs) const = default; 44*de74542cSJosh Lehan }; 45*de74542cSJosh Lehan 46*de74542cSJosh Lehan // Optional decorator class for each PID loop, to support logging 47*de74542cSJosh Lehan // Although this is a trivial class, it ended up needing the Six Horsemen 48*de74542cSJosh Lehan struct PidCoreLog 49*de74542cSJosh Lehan { 50*de74542cSJosh Lehan std::string nameOriginal; 51*de74542cSJosh Lehan std::string nameClean; 52*de74542cSJosh Lehan std::ofstream fileContext; 53*de74542cSJosh Lehan std::ofstream fileCoeffs; 54*de74542cSJosh Lehan std::chrono::milliseconds lastLog; 55*de74542cSJosh Lehan PidCoreContext lastContext; 56*de74542cSJosh Lehan bool moved; 57*de74542cSJosh Lehan 58*de74542cSJosh Lehan PidCoreLog() : 59*de74542cSJosh Lehan nameOriginal(), nameClean(), fileContext(), fileCoeffs(), lastLog(), 60*de74542cSJosh Lehan lastContext(), moved(false) 61*de74542cSJosh Lehan {} 62*de74542cSJosh Lehan 63*de74542cSJosh Lehan PidCoreLog(const PidCoreLog& copy) = delete; 64*de74542cSJosh Lehan 65*de74542cSJosh Lehan PidCoreLog& operator=(const PidCoreLog& copy) = delete; 66*de74542cSJosh Lehan 67*de74542cSJosh Lehan PidCoreLog(PidCoreLog&& move) 68*de74542cSJosh Lehan { 69*de74542cSJosh Lehan // Reuse assignment operator below 70*de74542cSJosh Lehan *this = std::move(move); 71*de74542cSJosh Lehan } 72*de74542cSJosh Lehan 73*de74542cSJosh Lehan PidCoreLog& operator=(PidCoreLog&& move) 74*de74542cSJosh Lehan { 75*de74542cSJosh Lehan if (this != &move) 76*de74542cSJosh Lehan { 77*de74542cSJosh Lehan *this = std::move(move); 78*de74542cSJosh Lehan 79*de74542cSJosh Lehan // Mark the moved object, so destructor knows it was moved 80*de74542cSJosh Lehan move.moved = true; 81*de74542cSJosh Lehan } 82*de74542cSJosh Lehan return *this; 83*de74542cSJosh Lehan } 84*de74542cSJosh Lehan 85*de74542cSJosh Lehan ~PidCoreLog() 86*de74542cSJosh Lehan { 87*de74542cSJosh Lehan // Do not close files if ownership was moved to another object 88*de74542cSJosh Lehan if (!moved) 89*de74542cSJosh Lehan { 90*de74542cSJosh Lehan fileContext.close(); 91*de74542cSJosh Lehan fileCoeffs.close(); 92*de74542cSJosh Lehan } 93*de74542cSJosh Lehan } 94*de74542cSJosh Lehan }; 95*de74542cSJosh Lehan 96*de74542cSJosh Lehan // Initializes logging files, call once per PID loop initialization 97*de74542cSJosh Lehan void LogInit(const std::string& name, pid_info_t* pidinfoptr); 98*de74542cSJosh Lehan 99*de74542cSJosh Lehan // Returns PidCoreLog pointer, or nullptr if this PID loop not being logged 100*de74542cSJosh Lehan PidCoreLog* LogPeek(const std::string& name); 101*de74542cSJosh Lehan 102*de74542cSJosh Lehan // Logs a line of logging, if different, or it has been long enough 103*de74542cSJosh Lehan void LogContext(PidCoreLog& pidLog, const std::chrono::milliseconds& msNow, 104*de74542cSJosh Lehan const PidCoreContext& coreLog); 105*de74542cSJosh Lehan 106*de74542cSJosh Lehan // Takes a timestamp, suitable for column 1 of logging file output 107*de74542cSJosh Lehan std::chrono::milliseconds LogTimestamp(void); 108*de74542cSJosh Lehan 109*de74542cSJosh Lehan } // namespace ec 110*de74542cSJosh Lehan } // namespace pid_control 111