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 loop() {
+void move(int motor, int speed, int dir)
-  // put your main code here, to run repeatedly:   
+{
 
-  //Controlling speed (0 = off and 255 = max speed):
+  if(motor == 1)
-  analogWrite(motor1EN, 100); //ENA pin
+  {
-  analogWrite(motor2EN, 200); //ENB pin
+    // Speed
-
+    analogWrite(motor1EN, speed); //ENA pin
-  analogWrite(motor3EN, 100); //ENA pin
+    // Direction
-  analogWrite(motor4EN, 200); //ENB pin
+    if(dir == 1)
-
+    {
-  //Controlling spin direction of motors:
+      digitalWrite(motor1pin1, HIGH);
-  digitalWrite(motor1pin1, HIGH);
+      digitalWrite(motor1pin2, LOW);
-  digitalWrite(motor1pin2, LOW);
+    }
-
+    else
-  digitalWrite(motor2pin1, HIGH);
+    {
-  digitalWrite(motor2pin2, LOW);
+      digitalWrite(motor1pin1, LOW);
-
+      digitalWrite(motor1pin2, HIGH);
-  digitalWrite(motor3pin1, HIGH);
+    }
-  digitalWrite(motor3pin2, LOW);
-
-  digitalWrite(motor4pin1, HIGH);
-  digitalWrite(motor4pin2, LOW);  
 
     
-  delay(1000);
+  }
+  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);
+    }
+  }
 
-  digitalWrite(motor1pin1, LOW);
+  return;
-  digitalWrite(motor1pin2, HIGH);
+}
-
+
-  digitalWrite(motor2pin1, LOW);
+void loop() {
-  digitalWrite(motor2pin2, HIGH);
+
-
+  move(1,200,1);
-  digitalWrite(motor3pin1, LOW);
+  move(2,200,1);
-  digitalWrite(motor3pin2, HIGH);
+  move(3,200,1);
-
+  move(4,200,1);
-  digitalWrite(motor4pin1, LOW);
+
-  digitalWrite(motor4pin2, HIGH);  
+  delay(2000);
-  delay(1000);
+
+  move(1,0,1);
+  move(2,0,1);
+  move(3,0,1);
+  move(4,0,1);
+
+  delay(2000);
 }