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