From c2108e9152a39fa6ceb1ec0be63b1682e9743fb5 Mon Sep 17 00:00:00 2001
From: jordojordo <jordonleach@gmail.com>
Date: Sun, 28 Jan 2024 05:58:47 -0500
Subject: [PATCH] Convert mower control to bldc with esc

---
 lawndon/drive.cpp |  2 ++
 lawndon/drive.h   |  4 ++--
 lawndon/motor.cpp | 35 +++++++++++++++--------------------
 lawndon/motor.h   |  7 ++-----
 4 files changed, 21 insertions(+), 27 deletions(-)

diff --git a/lawndon/drive.cpp b/lawndon/drive.cpp
index 08f6aa6..3e9a5f5 100644
--- a/lawndon/drive.cpp
+++ b/lawndon/drive.cpp
@@ -21,7 +21,9 @@ void Drive::setup() {
   delay(1);
 
   // Calibrate ESCs ( Needed for initial setup )
+  // Console.println(F("Calibrating left ESC"));
   // calibrateEsc(leftEsc);
+  // Console.println(F("Calibrating right ESC"));
   // calibrateEsc(rightEsc);
 
   // Arm ESCs
diff --git a/lawndon/drive.h b/lawndon/drive.h
index 84440b6..05cade5 100644
--- a/lawndon/drive.h
+++ b/lawndon/drive.h
@@ -10,8 +10,8 @@ extern Servo leftEsc;
 extern Servo rightEsc;
 
 // pin defs
-#define ESC_LEFT_PWM            3
-#define ESC_LEFT_POWER          4
+#define ESC_LEFT_PWM            4
+#define ESC_LEFT_POWER          3
 
 #define ESC_RIGHT_PWM           11
 #define ESC_RIGHT_POWER         10
diff --git a/lawndon/motor.cpp b/lawndon/motor.cpp
index 8d17d1c..7fc937d 100644
--- a/lawndon/motor.cpp
+++ b/lawndon/motor.cpp
@@ -2,41 +2,36 @@
 #include "controller.h"
 #include <Arduino.h>
 #include <IBusBM.h>
+#include <Servo.h>
 
 Motor motor;
+Servo mowerEsc;
 
 Motor::Motor() {}
 
 void Motor::setup() {
   Console.println(F("Initializing motor config"));
 
-  setupTimer4();
-  pinMode(motorEnaR, OUTPUT);
-  pinMode(motorEnaL, OUTPUT);
-  pinMode(motorPwmR, OUTPUT);
-  pinMode(motorPwmL, OUTPUT);
+  // Attach Mower ESC
+  Console.println(F("Attaching mower ESC"));
+  mowerEsc.attach(ESC_MOWER_PWM, 1000, 2000);
+  delay(1);
 
-  digitalWrite(motorEnaR, HIGH);
-  digitalWrite(motorEnaL, HIGH);
-  analogWrite(motorPwmR, LOW);
-  analogWrite(motorPwmL, LOW);
+  // Arm Mower ESC
+  Console.println(F("Arming mower ESC"));
+  mowerEsc.writeMicroseconds(1000);
+
+  // Set mower ESC pins
+  pinMode(ESC_MOWER_PWM, OUTPUT);
+  pinMode(ESC_MOWER_POWER, OUTPUT);
 
   Console.println(F("Motor setup complete"));
 }
 
 void Motor::loop() {  
-  int swdOut = map(controller.control_CH9_swd, 0, 1, 0, 255);
-
   if (controller.control_CH9_swd == 1) {
-    analogWrite(motorPwmR, swdOut);
+    mowerEsc.writeMicroseconds(2000);
   } else {
-    analogWrite(motorPwmR, swdOut);
+    mowerEsc.writeMicroseconds(1000);
   }
 }
-
-// Adjusts Timer4 for a frequency of 1kHz
-// [optional]: depends on the mower motor requirements
-void Motor::setupTimer4() {
-  TCCR4B = TCCR4B & 0b11111000 | 0x01; // Change prescaler
-  ICR4 = 32000; // Adjust the TOP value for 1kHz at 16MHz clock speed
-}
diff --git a/lawndon/motor.h b/lawndon/motor.h
index 4c64d4f..1df0e2d 100644
--- a/lawndon/motor.h
+++ b/lawndon/motor.h
@@ -5,10 +5,8 @@
 #include <Arduino.h>
 
 // Mower Motor pins
-#define motorEnaR 22
-#define motorEnaL 23
-#define motorPwmR 6
-#define motorPwmL 7
+#define ESC_MOWER_POWER 6
+#define ESC_MOWER_PWM 7
 
 class Motor {
 public:
@@ -16,7 +14,6 @@ class Motor {
 
   virtual void setup();
   virtual void loop();
-  virtual void setupTimer4();
 };
 
 extern Motor motor;