#include "MeMCore.h"

MeLineFollower lineFollower(PORT_1);
MeDCMotor motor1(9);
MeDCMotor motor2(10);

const uint8_t motorSpeed = 100;

void setup() {
  Serial.begin(9600);
}

void loop() {
  uint8_t sensorValue = lineFollower.readSensors();
  Serial.println(sensorValue);
  //S1_IN_S2_IN
  if (sensorValue == 0) { // Both sensors are on the black line
    // Move forward
    motor1.run(-motorSpeed);
    motor2.run(motorSpeed);
  } else if (sensorValue == 2) { 
    // Turn slightly right
    motor1.run(-motorSpeed);
    motor2.run(motorSpeed / 200);
  } else if (sensorValue == 1) {
    // Turn slightly left
    motor1.run(-motorSpeed / 200);
    motor2.run(motorSpeed);
  } else { // Both sensors are on the white surface (lost the line)
    motor1.stop();
    motor2.stop();
  }
}