From db1f8c410052b4976be853ef33c4d62254d3b076 Mon Sep 17 00:00:00 2001
From: mattmcw <matt@sixteenmillimeter.com>
Date: Wed, 21 Feb 2024 16:47:53 +0100
Subject: [PATCH] Make some sacrifices in the DriveMotor class to make it
 non-generalizable. Working towards porting timing functions from encoder
 proof of concept sketch.

---
 ino/contact_printer/ContactPrinter.cpp  |  4 ++-
 ino/contact_printer/ContactPrinter.h    |  4 +++
 ino/contact_printer/DriveMotor.cpp      | 34 +++++++++++++++++++------
 ino/contact_printer/DriveMotor.h        | 27 ++++++++++++++------
 ino/contact_printer/contact_printer.ino |  2 +-
 5 files changed, 53 insertions(+), 18 deletions(-)

diff --git a/ino/contact_printer/ContactPrinter.cpp b/ino/contact_printer/ContactPrinter.cpp
index 23505e0..0dae16c 100644
--- a/ino/contact_printer/ContactPrinter.cpp
+++ b/ino/contact_printer/ContactPrinter.cpp
@@ -119,6 +119,9 @@ bool ContactPrinter::IsRunning () {
 
 void ContactPrinter::Loop () {
 	timer = millis();
+	/*ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    	pos = posi;
+  	}*/
 	if (initialized) {
 		ButtonLoop();
 		if (running) {
@@ -128,4 +131,3 @@ void ContactPrinter::Loop () {
 		initialized = true;
 	}
 }
-
diff --git a/ino/contact_printer/ContactPrinter.h b/ino/contact_printer/ContactPrinter.h
index 6175a5b..20d654f 100644
--- a/ino/contact_printer/ContactPrinter.h
+++ b/ino/contact_printer/ContactPrinter.h
@@ -58,6 +58,7 @@ class ContactPrinter {
 	volatile bool initialized = false;
 	volatile bool running = false;
 
+
 	public:
 
 	ContactPrinter();
@@ -80,6 +81,9 @@ class ContactPrinter {
 	void ButtonLoop();
 
 	bool IsRunning ();
+
+	float CalculateFPS (long timeLength, uint32_t frames);
+	float CalculateRPM (long rotationLength);
 };
 
 #endif
diff --git a/ino/contact_printer/DriveMotor.cpp b/ino/contact_printer/DriveMotor.cpp
index 7479aa5..b30fe9e 100644
--- a/ino/contact_printer/DriveMotor.cpp
+++ b/ino/contact_printer/DriveMotor.cpp
@@ -4,14 +4,6 @@ DriveMotor::DriveMotor () {
 
 };
 
-DriveMotor::DriveMotor (uint8_t e_pin, uint8_t f_pin, uint8_t b_pin, uint8_t ea_pin, uint8_t eb_pin) {
-	enable_pin = e_pin;
-	forward_pin = f_pin;
-	backward_pin = b_pin;
-	encoder_a_pin = ea_pin;
-	encoder_b_pin = eb_pin;
-};
-
 void DriveMotor::Setup () {
 	pinMode(enable_pin, OUTPUT);
 	pinMode(forward_pin, OUTPUT);
@@ -31,6 +23,8 @@ void DriveMotor::Setup () {
 
 	digitalWrite(forward_pin, LOW);
 	digitalWrite(backward_pin, LOW);
+
+	//attachInterrupt(digitalPinToInterrupt(encoder_b_pin), ReadEncoder, RISING);
 }
 
 void DriveMotor::Start() {
@@ -51,6 +45,30 @@ void DriveMotor::SetSpeed(float speed) {
 	Serial.println(pwm_duty_cycle);
 }
 
+
+int64_t DriveMotor::posi = 0;
+/*
+void DriveMotor::ReadEncoder () {
+	int b = digitalRead(DriveMotor::encoder_b_pin);
+	if (b > 0) {
+		posi++;
+	} else {
+		posi--;
+	}
+}*/
+
 void DriveMotor::Loop () {
+	int64_t pos;
 	//monitor speed
+	/*ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    	pos = posi;
+  	}*/
+}
+
+float DriveMotor::CalculateFPS (long timeLength, uint32_t frames) {
+  return 1000.0 / ((float) timeLength / (float) frames);
+}
+
+float DriveMotor::CalculateRPM (long rotationLength) {
+  return 60000.0 / (float) (rotationLength);
 }
diff --git a/ino/contact_printer/DriveMotor.h b/ino/contact_printer/DriveMotor.h
index 3b8fb3a..599070d 100644
--- a/ino/contact_printer/DriveMotor.h
+++ b/ino/contact_printer/DriveMotor.h
@@ -8,13 +8,11 @@ class DriveMotor {
 	private:
 
 	//defaults are for EPS32 dev board
-	volatile uint8_t enable_pin = 26;
-	volatile uint8_t forward_pin = 27; //Clockwise
-	volatile uint8_t backward_pin = 14; //Counter-clockwise
-	volatile uint8_t encoder_a_pin = 33;
-	volatile uint8_t encoder_b_pin = 25;
-
-	volatile uint8_t pwm_duty_cycle = 0;
+	const uint8_t enable_pin = 26;
+	const uint8_t forward_pin = 27; //Clockwise
+	const uint8_t backward_pin = 14; //Counter-clockwise
+	const uint8_t encoder_a_pin = 33;
+	const uint8_t encoder_b_pin = 25;
 
 	const uint32_t pwm_frequency = 5000;
 	const uint8_t pwm_channel = 0;
@@ -26,19 +24,32 @@ class DriveMotor {
 	const uint32_t maxPulses = (int) round((float) ppr * ratio);
 	const uint8_t framesPerRotation = 18;
 
+	volatile uint8_t pwm_duty_cycle = 0;
+
+	static int64_t posi;
+
+	//measured
+	volatile float rpm = 0.0;
+	volatile float fps = 0.0;
+
+	//target
 	volatile float target_fps = 0.0;
 	volatile float target_rpm = 0.0;
 
 	public:
 
 	DriveMotor();
-	DriveMotor(uint8_t e_pin, uint8_t f_pin, uint8_t b_pin, uint8_t ea_pin, uint8_t eb_pin);
 	void Setup();
 	void Loop();
 	void Start();
 	void Stop();
 	void SetSpeed(float speed);
 
+	float CalculateFPS (long timeLength, uint32_t frames);
+	float CalculateRPM (long rotationLength);
+
+	protected: 
+
 	static void ReadEncoder();
 
 };
diff --git a/ino/contact_printer/contact_printer.ino b/ino/contact_printer/contact_printer.ino
index a4a3e39..a8247eb 100644
--- a/ino/contact_printer/contact_printer.ino
+++ b/ino/contact_printer/contact_printer.ino
@@ -11,7 +11,7 @@
  * 
  * 21 Takeup Picture Enable - set duty rate
  * 22 Takeup Direction A - Stock Clockwise, Picture Counter Clockwise
- * 23 Takeup Direction B  - Stock Counter Clockwise, Picture Clockwise
+ * 23 Takeup Direction B  - Stoc k Counter Clockwise, Picture Clockwise
  * 
  * 26 Drive Enable
  * 27 Drive Forward (Clockwise)