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