From bfa61563c09c69326f8cf754ff42fae8995810f2 Mon Sep 17 00:00:00 2001
From: dan <Dan.Dembinski@Gmail.com>
Date: Wed, 21 Sep 2022 22:26:23 -0400
Subject: [PATCH] Broke move logic into separate function

---
 src/main.cpp | 117 ++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 84 insertions(+), 33 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 30a75c4..f1d3137 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -34,46 +34,97 @@ void setup() {
   pinMode(motor3pin2, OUTPUT);
   pinMode(motor4pin1, OUTPUT);
   pinMode(motor4pin2, OUTPUT);
+
   pinMode(motor3EN, OUTPUT); 
   pinMode(motor4EN, OUTPUT);
 }
 
+void move(int motor, int speed, int dir)
+{
+
+  if(motor == 1)
+  {
+    // Speed
+    analogWrite(motor1EN, speed); //ENA pin
+    // Direction
+    if(dir == 1)
+    {
+      digitalWrite(motor1pin1, HIGH);
+      digitalWrite(motor1pin2, LOW);
+    }
+    else
+    {
+      digitalWrite(motor1pin1, LOW);
+      digitalWrite(motor1pin2, HIGH);
+    }
+
+    
+  }
+  else if(motor == 2)
+  {
+    // Speed
+    analogWrite(motor2EN, speed); //ENA pin
+    // Direction
+    if(dir == 1)
+    {
+      digitalWrite(motor2pin1, HIGH);
+      digitalWrite(motor2pin2, LOW);
+    }
+    else
+    {
+      digitalWrite(motor2pin1, LOW);
+      digitalWrite(motor2pin2, HIGH);
+    }
+  }
+  else if(motor == 3)
+  {
+    // Speed
+    analogWrite(motor3EN, speed); //ENA pin
+    // Direction
+    if(dir == 1)
+    {
+      digitalWrite(motor3pin1, HIGH);
+      digitalWrite(motor3pin2, LOW);
+    }
+    else
+    {
+      digitalWrite(motor3pin1, LOW);
+      digitalWrite(motor3pin2, HIGH);
+    }
+  }
+  else if(motor == 4)
+  {
+    // Speed
+    analogWrite(motor4EN, speed); //ENA pin
+    // Direction
+    if(dir == 1)
+    {
+      digitalWrite(motor4pin1, HIGH);
+      digitalWrite(motor4pin2, LOW);
+    }
+    else
+    {
+      digitalWrite(motor4pin1, LOW);
+      digitalWrite(motor4pin2, HIGH);
+    }
+  }
+
+  return;
+}
+
 void loop() {
-  // put your main code here, to run repeatedly:   
 
-  //Controlling speed (0 = off and 255 = max speed):
-  analogWrite(motor1EN, 100); //ENA pin
-  analogWrite(motor2EN, 200); //ENB pin
+  move(1,200,1);
+  move(2,200,1);
+  move(3,200,1);
+  move(4,200,1);
 
-  analogWrite(motor3EN, 100); //ENA pin
-  analogWrite(motor4EN, 200); //ENB pin
+  delay(2000);
 
-  //Controlling spin direction of motors:
-  digitalWrite(motor1pin1, HIGH);
-  digitalWrite(motor1pin2, LOW);
+  move(1,0,1);
+  move(2,0,1);
+  move(3,0,1);
+  move(4,0,1);
 
-  digitalWrite(motor2pin1, HIGH);
-  digitalWrite(motor2pin2, LOW);
-
-  digitalWrite(motor3pin1, HIGH);
-  digitalWrite(motor3pin2, LOW);
-
-  digitalWrite(motor4pin1, HIGH);
-  digitalWrite(motor4pin2, LOW);  
-
-  
-  delay(1000);
-
-  digitalWrite(motor1pin1, LOW);
-  digitalWrite(motor1pin2, HIGH);
-
-  digitalWrite(motor2pin1, LOW);
-  digitalWrite(motor2pin2, HIGH);
-
-  digitalWrite(motor3pin1, LOW);
-  digitalWrite(motor3pin2, HIGH);
-
-  digitalWrite(motor4pin1, LOW);
-  digitalWrite(motor4pin2, HIGH);  
-  delay(1000);
+  delay(2000);
 }
\ No newline at end of file