Broke move logic into separate function

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):
+  move(1,200,1);
-  analogWrite(motor1EN, 100); //ENA pin
+  move(2,200,1);
-  analogWrite(motor2EN, 200); //ENB pin
+  move(3,200,1);
+  move(4,200,1);
 
-  analogWrite(motor3EN, 100); //ENA pin
+  delay(2000);
-  analogWrite(motor4EN, 200); //ENB pin
 
-  //Controlling spin direction of motors:
+  move(1,0,1);
-  digitalWrite(motor1pin1, HIGH);
+  move(2,0,1);
-  digitalWrite(motor1pin2, LOW);
+  move(3,0,1);
+  move(4,0,1);
 
-  digitalWrite(motor2pin1, HIGH);
+  delay(2000);
-  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);
 }