diff --git a/include/settings.h b/include/settings.h
new file mode 100644
index 0000000000000000000000000000000000000000..5bc71fbd6ecf88e560742ec6be158379e6e173a5
--- /dev/null
+++ b/include/settings.h
@@ -0,0 +1,28 @@
+// From top to bottom of motor driver (bottom is big capacitor)
+#define MOTOR_LEFT_PIN_2 D5
+#define MOTOR_LEFT_PIN_1 D6
+#define MOTOR_RIGHT_PIN_2 D9
+#define MOTOR_RIGHT_PIN_1 D10
+
+
+#define IR_SENSOR_LEFT_PIN A0
+#define IR_SENSOR_RIGHT_PIN A1
+
+#define DEBOUNCE_TIME_MS 50
+
+#define LEFT_MOTOR_SPEED 50
+#define RIGHT_MOTOR_SPEED 50
+#define LEFT_IR_THRESHOLD 500
+#define RIGHT_IR_THRESHOLD 500
+
+
+// mode 0(default) - break, 1 - coast
+void motorLeftForward(int speed, bool mode = 0);
+void motorLeftBackwards(int speed, bool mode = 0);
+void motorRightForward(int speed, bool mode = 0);
+void motorRightBackwards(int speed, bool mode = 0);
+
+typedef enum {
+    MODE_BREAK,
+    MODE_COAST
+} MotorMode;
diff --git a/src/main.cpp b/src/main.cpp
index e2df9ccaee50c7d670bdf5fa4fb61c9addb3cef9..9cd8ebc45351f4ca343fcb3f3aab874a449f4dd1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,18 +1,12 @@
 #include <Arduino.h>
 #include <Wire.h>
+#include "settings.h"
 #include "Adafruit_TCS34725.h"
 
-#define MOTOR_PIN_1 D6
-#define MOTOR_PIN_2 D5
-#define DEBOUNCE_TIME_MS 50
-
-// mode 0(default) - break, 1 - coast
-void motorForward(int speed, bool mode = 0);
-void motorBackwards(int speed, bool mode = 0);
-
 // Color sensor
 // Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
 
+
 bool forward = false;
 bool prev_button_state = true; // nominal high
 int prev_button_press_time = 0;
@@ -20,14 +14,18 @@ int prev_button_press_time = 0;
 void setup() {
   Serial.begin(115200);
 
-  pinMode(A0, INPUT_ANALOG);
-  pinMode(MOTOR_PIN_1, OUTPUT);
-  pinMode(MOTOR_PIN_2, OUTPUT);
+  pinMode(IR_SENSOR_LEFT_PIN, INPUT_ANALOG);
+  pinMode(IR_SENSOR_RIGHT_PIN, INPUT_ANALOG);
+
+  pinMode(MOTOR_LEFT_PIN_1, OUTPUT);
+  pinMode(MOTOR_LEFT_PIN_2, OUTPUT);
+  pinMode(MOTOR_RIGHT_PIN_1, OUTPUT);
+  pinMode(MOTOR_RIGHT_PIN_2, OUTPUT);
   pinMode(USER_BTN, INPUT);
 
   // Color sensor setup
   // if (tcs.begin()) {
-  //   //Serial.println("Found sensor");
+  //   Serial.println("Found sensor");
   // } else {
   //   Serial.println("No TCS34725 found ... check your connections");
   //   while (1); // halt!
@@ -46,14 +44,41 @@ void loop() {
   }
   prev_button_state = current_button_state;
 
-  int motor_speed = map(analogRead(A0), 0, 1023, 0, 255);
+  // uint16_t ir_left_value = analogRead(IR_SENSOR_LEFT_PIN);
+  // uint16_t ir_right_value = analogRead(IR_SENSOR_RIGHT_PIN);
+  // Serial.print(ir_left_value);
+  // Serial.print(" ");
+  // Serial.println(ir_right_value);
+
+
+  // For testing motor
+  // int motor_speed = map(ir_left_value, 0, 1023, 0, 255);
   // Serial.println(motor_speed);
-  if (forward) {
-    motorForward(motor_speed);
+
+  // Enable line following
+  /*
+  if (ir_left_value < LEFT_IR_THRESHOLD) {
+    motorLeftForward(LEFT_MOTOR_SPEED);
   } else {
-    motorBackwards(motor_speed);
+    motorLeftForward(0);
   }
 
+  if (ir_right_value < RIGHT_IR_THRESHOLD) {
+    motorRightForward(RIGHT_MOTOR_SPEED);
+  } else {
+    motorRightForward(0);
+  }
+  */
+
+  // For testing motor
+  // if (forward) {
+  //   motorLeftForward(motor_speed);
+  //   motorRightForward(motor_speed);
+  // } else {
+  //   motorLeftBackwards(motor_speed);
+  //   motorRightBackwards(motor_speed);
+  // }
+
   // Color sensor code
   // float red, green, blue;
   // tcs.setInterrupt(false);  // turn on LED
@@ -64,40 +89,55 @@ void loop() {
   // Serial.print("R:"); Serial.print(int(red));
   // Serial.print("\tG:"); Serial.print(int(green));
   // Serial.print("\tB:"); Serial.print(int(blue));
- 
   // Serial.print("\n");
 }
 
-void motorForward(int speed, bool mode) {
+// Left motor
+void motorLeftForward(int speed, bool mode) {
   if (!mode) {
     // Break
-    analogWrite(MOTOR_PIN_2, 255);
-    analogWrite(MOTOR_PIN_1, speed);
+    analogWrite(MOTOR_LEFT_PIN_2, 255);
+    analogWrite(MOTOR_LEFT_PIN_1, speed);
   } else {
     // Coast
-    analogWrite(MOTOR_PIN_2, 0);
-    analogWrite(MOTOR_PIN_1, 255-speed);
+    analogWrite(MOTOR_LEFT_PIN_2, 0);
+    analogWrite(MOTOR_LEFT_PIN_1, 255-speed);
+  }
+}
+
+void motorLeftBackwards(int speed, bool mode) {
+  if (!mode) {
+    // Break
+    analogWrite(MOTOR_LEFT_PIN_1, 255);
+    analogWrite(MOTOR_LEFT_PIN_2, speed);
+  } else {
+    // Coast
+    analogWrite(MOTOR_LEFT_PIN_1, 0);
+    analogWrite(MOTOR_LEFT_PIN_2, 255-speed);
   }
-  
-  Serial.print("Forward: ");
-  Serial.print(speed);
-  Serial.print(" ");
-  Serial.println(mode);
 }
 
-void motorBackwards(int speed, bool mode) {
+// Right motor
+void motorRightForward(int speed, bool mode) {
   if (!mode) {
     // Break
-    analogWrite(MOTOR_PIN_1, 255);
-    analogWrite(MOTOR_PIN_2, speed);
+    analogWrite(MOTOR_RIGHT_PIN_2, 255);
+    analogWrite(MOTOR_RIGHT_PIN_1, speed);
   } else {
     // Coast
-    analogWrite(MOTOR_PIN_1, 0);
-    analogWrite(MOTOR_PIN_2, 255-speed);
+    analogWrite(MOTOR_RIGHT_PIN_2, 0);
+    analogWrite(MOTOR_RIGHT_PIN_1, 255-speed);
   }
+}
 
-  Serial.print("Backwards: ");
-  Serial.print(speed);
-  Serial.print(" ");
-  Serial.println(mode);
+void motorRightBackwards(int speed, bool mode) {
+  if (!mode) {
+    // Break
+    analogWrite(MOTOR_RIGHT_PIN_1, 255);
+    analogWrite(MOTOR_RIGHT_PIN_2, speed);
+  } else {
+    // Coast
+    analogWrite(MOTOR_RIGHT_PIN_1, 0);
+    analogWrite(MOTOR_RIGHT_PIN_2, 255-speed);
+  }
 }
\ No newline at end of file