From 1056e81b82addfebf7663e0252838a741ebe467a Mon Sep 17 00:00:00 2001
From: mattmcw <matt@sixteenmillimeter.com>
Date: Sat, 17 Feb 2024 13:05:43 +0000
Subject: [PATCH] Fill out EncoderMotor methods and add variables to be used to
 calculate position + speed and used for PID. TODO: rename class because it is
 not a generic EncoderMotor class, should instead be DriveMotor.

---
 ino/contact_printer/ContactPrinter.cpp |  1 +
 ino/contact_printer/ContactPrinter.h   |  2 ++
 ino/contact_printer/EncoderMotor.cpp   | 19 +++++++++++++++++--
 ino/contact_printer/EncoderMotor.h     |  8 +++++++-
 4 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/ino/contact_printer/ContactPrinter.cpp b/ino/contact_printer/ContactPrinter.cpp
index f904086..8aedbae 100644
--- a/ino/contact_printer/ContactPrinter.cpp
+++ b/ino/contact_printer/ContactPrinter.cpp
@@ -106,5 +106,6 @@ bool ContactPrinter::IsRunning () {
 }
 
 void ContactPrinter::Loop () {
+	timer = millis();
 	drive_motor.Loop();
 }
\ No newline at end of file
diff --git a/ino/contact_printer/ContactPrinter.h b/ino/contact_printer/ContactPrinter.h
index 99fc1ef..cf9e5d4 100644
--- a/ino/contact_printer/ContactPrinter.h
+++ b/ino/contact_printer/ContactPrinter.h
@@ -26,6 +26,8 @@ class ContactPrinter {
 	const uint8_t takeup_stock_pwm_channel = 2;
 	const uint8_t pwm_resolution = 8;
 
+	volatile long timer = 0;
+
 	volatile float drive_speed = 1.0;  //calculated rpm
 	volatile float takeup_speed = 1.0; //estimated rpm
 
diff --git a/ino/contact_printer/EncoderMotor.cpp b/ino/contact_printer/EncoderMotor.cpp
index a50c65a..b3b73a2 100644
--- a/ino/contact_printer/EncoderMotor.cpp
+++ b/ino/contact_printer/EncoderMotor.cpp
@@ -25,6 +25,21 @@ void EncoderMotor::Setup () {
 	digitalWrite(backward_pin, LOW);
 }
 
-void EncoderMotor::Loop () {
-	
+void EncoderMotor::Start() {
+	ledcWrite(pwm_channel, pwm_duty_cycle);
+	digitalWrite(forward_pin, HIGH);
+	digitalWrite(backward_pin, LOW);
+}
+void EncoderMotor::Stop() {
+	pwm_duty_cycle = 0;
+	digitalWrite(forward_pin, LOW);
+	digitalWrite(backward_pin, LOW);
+	ledcWrite(pwm_channel, pwm_duty_cycle);
+}
+void EncoderMotor::SetSpeed() {
+	pwm_duty_cycle = 255;
+}
+
+void EncoderMotor::Loop () {
+	//monitor speed
 }
diff --git a/ino/contact_printer/EncoderMotor.h b/ino/contact_printer/EncoderMotor.h
index b6d1d3b..65d5157 100644
--- a/ino/contact_printer/EncoderMotor.h
+++ b/ino/contact_printer/EncoderMotor.h
@@ -14,12 +14,18 @@ class EncoderMotor {
 	volatile uint8_t encoder_a_pin = 27;
 	volatile uint8_t encoder_b_pin = 26;
 
-	volatile uint8_t pwm_duty_cycle = 255;
+	volatile uint8_t pwm_duty_cycle = 0;
 
 	const uint32_t pwm_frequency = 30000;
 	const uint8_t pwm_channel = 0;
 	const uint8_t pwm_resolution = 8;
 
+	const uint8_t ppr = 11;
+	const float ratio = 187.0 / 3.0;
+	const uint32_t maxPulses = (int) round((float) ppr * ratio);
+	const uint8_t speed = 255;
+	const uint8_t framesPerRotation = 18;
+
 	public:
 
 	EncoderMotor();