Mark based Line Following algorithm

LineFollower.cpp

struct Mark {
  float speed;
  float acceleration;
  uint16_t duration;
};

Mark marks[] = {
  {100.0, 100, 300},
  {60.0,  100, 100},
  {60.0,  100, 100},
  {60.0,  100, 100},
  {60.0,  100, 100},
  {60.0,  100, 100},
  {0.0,   200, 10000},
  // Aqui
};

// Accelerate Speed
// float targetSpeed = 0;
float currentSpeed = 0;
float speedAcceleration = 100;

int currentMark = 0;
uint16_t position = 0;
uint16_t lastMark = 0;

float lastT = 0
void loop() {
  // Save dt
  float now = Timer.read()
  float dt = now - lastT;
  lastT = now

  if (dt > 0.005) {
    flashLed();
    return;
  }

  position = readEncoder();

  // Get current mark
  Mark mark = marks[currentMark];

  // Check if changed mark
  if (position - lastMark > mark.duration) {
    lastMark += mark.duration;
    currentMark++;
    mark = marks[currentMark];
  }
  
  // Base speed to set
  float targetSpeed = mark.speed;
  float targetDelta = (targetSpeed - currentSpeed);
  float speedDelta = speedAcceleration * dt;

  // Revert target if lt 0
  if (targetDelta < 0)
    speedDelta = max(-speedDelta, targetDelta);
  else
    speedDelta = min( speedDelta, targetDelta);

  currentSpeed += speedDelta;
}