Adjust timing for motor pwm - fix initial logging

lawndon/controller.cpp

@@ -20,9 +20,11 @@ int Controller::readChannel(byte channelInput, int minLimit, int maxLimit,
 };
 
 void Controller::setup() {
-  Console.println(F("Sending Controller Config"));
+  Console.println(F("Sending controller config"));
   ControllerPort.begin(CONTROLLER_BAUDRATE);
   ibus.begin(ControllerPort);
+
+  Console.println(F("Controller setup complete"));
 }
 
 void Controller::loop() {

lawndon/drive.cpp

@@ -11,22 +11,35 @@ Drive::Drive() {};
 
 void Drive::setup() {
   Console.begin(CONSOLE_BAUDRATE);
-  Console.println(F("SETUP"));
-
+  Console.println(F("__________SETUP__________"));
+  Console.println(F("Initializing drive config"));
+
   // Attach ESCs
+  Console.println(F("Attaching ESCs"));
   leftEsc.attach(ESC_LEFT_PWM, 1000, 2000);
   rightEsc.attach(ESC_RIGHT_PWM, 1000, 2000);
   delay(1);
 
-  // Calibrate ESCs
-  calibrateEsc(leftEsc);
-  calibrateEsc(rightEsc);
+  // Calibrate ESCs ( Needed for initial setup )
+  // calibrateEsc(leftEsc);
+  // calibrateEsc(rightEsc);
+
+  // Arm ESCs
+  Console.println(F("Arming left ESC"));
+  armEsc(leftEsc);
+
+  Console.println(F("Arming right ESC"));
+  armEsc(rightEsc);
+  delay(1);
+
 
   // Set ESC pins
   pinMode(ESC_LEFT_PWM, OUTPUT);
   pinMode(ESC_LEFT_POWER, OUTPUT);
   pinMode(ESC_RIGHT_PWM, OUTPUT);
   pinMode(ESC_RIGHT_POWER, OUTPUT);
+
+  Console.println(F("Drive setup complete"));
 }
 
 void Drive::loop() {
@@ -76,6 +89,11 @@ void Drive::calibrateEsc(Servo esc) {
   delay(ESC_ARM_TIME);
 }
 
+// Initialize ESC
+void Drive::armEsc(Servo esc) {
+  esc.writeMicroseconds(ESC_ARM_SIGNAL);
+}
+
 // Controls supplied ESC by writing speed in microseconds
 void Drive::controlDriveMotor(int speed, Servo esc) {
   if (isDeadzone(speed)) {

lawndon/drive.h

@@ -39,6 +39,7 @@ class Drive {
   virtual void loop();
 
   virtual void calibrateEsc(Servo esc);
+  virtual void armEsc(Servo esc);
   virtual void controlDriveMotor(int speed, Servo esc);
   virtual bool isDeadzone(int speed);
 };

lawndon/lawndon.ino

@@ -9,6 +9,8 @@ void setup() {
   drive.setup();
   controller.setup();
   motor.setup();
+
+  Console.println(F("__________BEGIN__________"));
 }
 
 void loop() {

lawndon/motor.cpp

@@ -8,24 +8,35 @@ Motor motor;
 Motor::Motor() {}
 
 void Motor::setup() {
+  Console.println(F("Initializing motor config"));
+
+  setupTimer4();
   pinMode(motorEnaR, OUTPUT);
   pinMode(motorEnaL, OUTPUT);
   pinMode(motorPwmR, OUTPUT);
   pinMode(motorPwmL, OUTPUT);
 
-  digitalWrite(motorPwmR, LOW);
-  digitalWrite(motorPwmL, HIGH);
+  digitalWrite(motorEnaR, HIGH);
+  digitalWrite(motorEnaL, HIGH);
+  analogWrite(motorPwmR, LOW);
+  analogWrite(motorPwmL, LOW);
+
+  Console.println(F("Motor setup complete"));
 }
 
 void Motor::loop() {  
   int swdOut = map(controller.control_CH9_swd, 0, 1, 0, 255);
 
-  // Set motor 
-  analogWrite(motorEnaR, swdOut);
-
   if (controller.control_CH9_swd == 1) {
-    analogWrite(motorPwmR, HIGH);
+    analogWrite(motorPwmR, swdOut);
   } else {
-    analogWrite(motorPwmR, LOW);
+    analogWrite(motorPwmR, swdOut);
   }
-}
+}
+
+// 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
+}

lawndon/motor.h

@@ -5,18 +5,18 @@
 #include <Arduino.h>
 
 // Mower Motor pins
-#define motorEnaR 5
-#define motorEnaL 6
-#define motorVcc 7
-#define motorPwmR 22
-#define motorPwmL 23 
+#define motorEnaR 22
+#define motorEnaL 23
+#define motorPwmR 6
+#define motorPwmL 7
 
 class Motor {
 public:
   Motor();
 
   virtual void setup();
   virtual void loop();
+  virtual void setupTimer4();
 };
 
 extern Motor motor;