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