#include <TaskScheduler.h>
#include "Scheduler.h"
#include "Gates.h"
#include "Button.h"
#include "Blockdetection.h"
#include <Wire.h>

#define SERVO_FREQ 333

void setup(){
  Serial.begin(115200);

  // Setup der Lichtschranke  
  pinMode(pin_sensor, INPUT);
  attachInterrupt(digitalPinToInterrupt(pin_sensor), lightgate, RISING);

  // Setup der Zeiten
  convertLenghtToTime();

  // Setup des Schedulers
  runner.init();
  runner.addTask(taskStarter);
  runner.addTask(taskDetectBlock);
  runner.addTask(taskCheckColour);

  runner.addTask(taskStarterGate0);
  runner.addTask(taskGate0);
  runner.addTask(taskStarterGate1);
  runner.addTask(taskGate1);

  taskStarter.setIterations(1);
  taskStarter.enable();
}

void loop(){
  runner.execute();
}