   motor.setSpeed(speed);
 
 
-/*
-  TODO: Battery Protection
+  /* TODO: Battery Protection
   batteryVoltage = getBatteryVoltage();
-  while (batteryVoltage > 3 && batteryVoltage < 11.4) {
+  while (smoothBattery.getValue() > 3 && smoothBattery.getValue() < 11.4) {
     changeSpeed(currentDirection, speed, 0);
-    batteryVoltage = getBatteryVoltage();
-
+    motor.changeSpeed(0);
     delay(1000);
   }
   */
 
+  // Does the gear doesn't match with the motor gear?
+  if ((gear.isForwards() && !motor.isForwards()) ||
+      (gear.isBackwards() && !motor.isBackwards())) {
 
-  // Remember to switch forwards
-  if (switchForward.fell() && !motor.isForwards()) {
-    newDirection = Motor::DIRECTION_FORWARD;
-    Serial.println(">>>>>>>>>>> Forward!");
+    // Change gear only, if speed is below a configured speed
+    if (speed < SPEED_MIN_DIRECTION_CHANGE) {
+      motor.changeSpeed(0, 10);
+      motor.setDirection(gear.getGear());
+      motor.changeSpeed(speed, 10);
+    }
+   
   }
-
-  // Remember to switch backwards
-  if (switchBackward.fell() && !motor.isBackwards()) {
-    newDirection = Motor::DIRECTION_BACKWARD;
-    Serial.println(">>>>>>>>>> Backward!");
-  }
-
-
-  // Switch direction if speed is low enough
-  if (newDirection && speed < SPEED_MIN_DIRECTION_CHANGE) {
-
-    motor.changeSpeed(0, 10);
-    motor.setDirection(newDirection);
-    motor.changeSpeed(speed, 10);
-    
-    newDirection = DIRECTION_NONE;
-  }
-
   
-  switchForward.update();
-  switchBackward.update();
-
   updateDisplay();
 }


  lcd.setCursor(0, 1);
  lcd.print("Bat: ");
  lcd.print(batteryVoltage);
  lcd.print("V   ");
}

void loop() {
  uint8_t speed = 0;
  uint8_t currentGas = gas.getValue();
  uint8_t currentBatteryVoltage = getBatteryVoltage();
  
  smoothGas.update(currentGas);

  // Different max speed, when driving backwards
  if (motor.isForwards()) {

#include <Arduino.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include "Motor.h"
#include "Battery.h"
#include "GasPedal.h"
#include "Gear.h"
#include "ExponentialSmoothing.h"
#include "settings.h"

Motor motor = Motor(PIN_ENABLE_R, PIN_PWM_R, PIN_ENABLE_L, PIN_PWM_L);
Gear gear = Gear(PIN_SWITCH_FORWARD, PIN_SWITCH_BACKWARD);
GasPedal gas = GasPedal(PIN_GAS_PEDAL, GAS_VALUE_MIN, GAS_VALUE_MAX);
Battery battery = Battery(PIN_BATTERY_VOLTAGE);

ExponentialSmoothing smoothGas = ExponentialSmoothing();
ExponentialSmoothing smoothBattery = ExponentialSmoothing();

LiquidCrystal_I2C lcd(0x27, 16, 2);

