on hold

control/control.ino

@@ -21,14 +21,14 @@ void loop() {
   //(0,0)
   gyro.update();
   goForward();
-  delay(6000);
+  delay(2000);
   stop();
 
   //(10,0)
   
-  float error;
+  float error = 100;
   float d_target = 90;
-  float p;
+  float p = 0.5;
   float v_e;
   float v_r;
   

lineFollower/lineFollower.ino

@@ -0,0 +1,81 @@
+#include "MeMCore.h"
+
+MeDCMotor leftMotor(9);
+MeDCMotor rightMotor(10);
+MeLineFollower lineFinder(PORT_2); /* Line Finder module can only be connected to PORT_3, PORT_4, PORT_5, PORT_6 of base shield. */
+
+uint8_t motorSpeed = 50;
+
+void setup()
+{
+  Serial.begin(9600);
+}
+
+void loop()
+{
+  int sensorState = lineFinder.readSensors();
+  goForward();
+  switch(sensorState)
+  {
+    case S1_IN_S2_IN:
+      Serial.println("Sensor 1 and 2 are inside of black line"); 
+      goForward();
+    break;
+    case S1_IN_S2_OUT:
+      Serial.println("Sensor 2 is outside of black line");
+      goLeft();
+      goForward();
+    break;
+    case S1_OUT_S2_IN:
+      Serial.println("Sensor 1 is outside of black line");
+      goRight();
+      goForward();
+    break;
+    case S1_OUT_S2_OUT: 
+      Serial.println("Sensor 1 and 2 are outside of black line"); 
+      stop();
+    break;
+    default: break;
+  }
+  delay(200);
+}
+
+void goForward(){
+
+  leftMotor.run(-motorSpeed);
+  rightMotor.run(motorSpeed);
+  delay(1000);  
+
+}
+
+void goBackward(){
+
+  leftMotor.run(motorSpeed);
+  rightMotor.run(-motorSpeed);
+  delay(1000);  
+
+}
+
+void goLeft(){
+
+  leftMotor.run(motorSpeed);
+  rightMotor.run(motorSpeed);
+  delay(1000);
+
+}
+
+void goRight(){
+
+  leftMotor.run(-motorSpeed);
+  rightMotor.run(-motorSpeed);
+  delay(1000);
+
+}
+
+void stop(){
+
+  leftMotor.stop();
+  rightMotor.stop();
+  delay(1000);
+
+}