From b6eb4e431f97b8687df3838cd1357d04f076f9e0 Mon Sep 17 00:00:00 2001
From: Lee Bousfield <ljbousfield@gmail.com>
Date: Sat, 20 Jun 2015 15:36:07 -0600
Subject: [PATCH] Added code to wrap and log PIDSource and PIDOutput

---
 src/Commands/Drive/DriveStraight.h |  4 +-
 src/Common/PIDLogger.cpp           | 72 ++++++++++++++++++++++++++++++
 src/Common/PIDLogger.h             | 35 +++++++++++++++
 src/Subsystems/Drive.cpp           |  7 ++-
 4 files changed, 115 insertions(+), 3 deletions(-)
 create mode 100644 src/Common/PIDLogger.cpp
 create mode 100644 src/Common/PIDLogger.h

diff --git a/src/Commands/Drive/DriveStraight.h b/src/Commands/Drive/DriveStraight.h
index cefa300..7232e5e 100644
--- a/src/Commands/Drive/DriveStraight.h
+++ b/src/Commands/Drive/DriveStraight.h
@@ -12,6 +12,7 @@
 #include "Subsystems/Drive.h"
 #include <PIDSource.h>
 #include <PIDOutput.h>
+#include "Common/PIDLogger.h"
 
 namespace tator {
 
@@ -26,8 +27,9 @@ class DriveStraight: public CommandBase, public PIDSource, public PIDOutput {
 		speed = config["speed"].as<double>();
 		distance = config["distance"].as<double>();
 		YAML::Node pid_ = config["PID"];
+		PIDLogger* pidLogger = new PIDLogger(this, this, name);
 		pid = new PIDController(pid_["P"].as<double>(), pid_["I"].as<double>(),
-				pid_["D"].as<double>(), pid_["F"].as<double>(), this, this);
+				pid_["D"].as<double>(), pid_["F"].as<double>(), pidLogger, pidLogger);
 	}
 
 	static std::string GetBaseName() {
diff --git a/src/Common/PIDLogger.cpp b/src/Common/PIDLogger.cpp
new file mode 100644
index 0000000..bc39faa
--- /dev/null
+++ b/src/Common/PIDLogger.cpp
@@ -0,0 +1,72 @@
+/*
+ * PIDLogger.cpp
+ *
+ *  Created on: Jun 20, 2015
+ *      Author: lee
+ */
+
+#include <Common/PIDLogger.h>
+#include <sys/stat.h>
+#include <dirent.h>
+#include <errno.h>
+#include <string.h>
+#include <stdarg.h>
+
+namespace tator {
+
+PIDLogger::PIDLogger(PIDSource* in, PIDOutput* out, std::string name) {
+	// Stolen from USBManager
+	if (log == nullptr)
+		log = new Logger("PIDLogger");
+	log->Info("Opening PID log directory...");
+	DIR* dir;
+	if ((dir = opendir(kLogDirectory)) == nullptr) {
+		if (errno == ENOENT) {
+			if (!mkdir(kLogDirectory, 0755)) {
+				log->Error("Creating PID log directory: %s", strerror(errno));
+				return;
+			} else {
+				log->Info("Created PID log directory");
+			}
+		} else {
+			log->Error("Opening PID log directory: %s", strerror(errno));
+			return;
+		}
+	} else {
+		closedir(dir);
+	}
+	this->in = in;
+	this->out = out;
+	time_t rawtime;
+	struct tm* timeinfo;
+	char buf[80];
+	time(&rawtime);
+	timeinfo = localtime(&rawtime);
+	strftime(buf, sizeof(buf), ("PIDlog_" + name + " %C %a %b %d %H.%M.%S.txt").c_str(), timeinfo);
+	char fname[120];
+	strcpy(fname, kLogDirectory);
+	strcat(fname, buf);
+	log->Info("Opening log file: \"%s\"", fname);
+	file = fopen(fname, "w");
+	if (file == nullptr) {
+		log->Error("Opening log file: %s", strerror(errno));
+		return;
+	}
+}
+
+PIDLogger::~PIDLogger() {
+}
+
+
+double PIDLogger::PIDGet() {
+	double get = in->PIDGet();
+	fprintf(file, "g%f\n", get);
+	return get;
+}
+
+void PIDLogger::PIDWrite(float output) {
+	fprintf(file, "g%f\n", output);
+	return out->PIDWrite(output);
+}
+
+}
diff --git a/src/Common/PIDLogger.h b/src/Common/PIDLogger.h
new file mode 100644
index 0000000..76da480
--- /dev/null
+++ b/src/Common/PIDLogger.h
@@ -0,0 +1,35 @@
+/*
+ * PIDLogger.h
+ *
+ *  Created on: Jun 20, 2015
+ *      Author: lee
+ */
+
+#ifndef PIDLOGGER_H_
+#define PIDLOGGER_H_
+
+#include <PIDOutput.h>
+#include <PIDSource.h>
+#include <cstdio>
+#include <string>
+#include "Logger.h"
+
+namespace tator {
+
+class PIDLogger: public PIDSource, public PIDOutput {
+public:
+	PIDLogger(PIDSource* in, PIDOutput* out, std::string name);
+	virtual ~PIDLogger();
+	virtual double PIDGet() override;
+	virtual void PIDWrite(float output) override;
+private:
+	PIDSource* in;
+	PIDOutput* out;
+	constexpr const static char* kLogDirectory = "/media/sda1/PIDlogs/";
+	Logger* log;
+	FILE* file;
+};
+
+}
+
+#endif /* PIDLOGGER_H_ */
diff --git a/src/Subsystems/Drive.cpp b/src/Subsystems/Drive.cpp
index 46e508d..025078a 100644
--- a/src/Subsystems/Drive.cpp
+++ b/src/Subsystems/Drive.cpp
@@ -6,6 +6,7 @@
 
 #include "Drive.h"
 #include "Common/Tester/ManualTester.h"
+#include "Common/PIDLogger.h"
 
 namespace tator {
 
@@ -19,10 +20,12 @@ Drive::Drive(YAML::Node config) :
 	encoderR = new Encoder(conEncR[0].as<int>(), conEncR[1].as<int>());
 	YAML::Node pidl = config["PID"]["L"];
 	YAML::Node pidr = config["PID"]["R"];
+	PIDLogger* pidLoggerL = new PIDLogger(encoderL, driveL, "DriveLeft");
 	pidL = new PIDController(pidl["P"].as<double>(), pidl["I"].as<double>(),
-			pidl["D"].as<double>(), pidl["F"].as<double>(), encoderL, driveL);
+			pidl["D"].as<double>(), pidl["F"].as<double>(), pidLoggerL, pidLoggerL);
+	PIDLogger* pidLoggerR = new PIDLogger(encoderR, driveR, "DriveRight");
 	pidR = new PIDController(pidr["P"].as<double>(), pidr["I"].as<double>(),
-			pidr["D"].as<double>(), pidr["F"].as<double>(), encoderR, driveR);
+			pidr["D"].as<double>(), pidr["F"].as<double>(), pidLoggerR, pidLoggerR);
 	maxRPS = config["maxRPS"].as<double>();
 
 	encoderL->SetDistancePerPulse(1.0 / 360.0);