diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..9a6845d
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,6 @@
+{
+    "cSpell.ignoreWords": [
+        "ledc",
+        "write"
+    ]
+}
\ No newline at end of file
diff --git a/src/config.h b/src/config.h
index a1915fb..8dc2c97 100644
--- a/src/config.h
+++ b/src/config.h
@@ -16,8 +16,12 @@
 #define FORWARD 1
 #define BACKWARD 2
 
-//PWM Configs
-#define PWM_FREQ 5000
+//MotorControl config
+#define PWM_FREQ 16000
 #define PWM_RES 8
 #define PWM_CHANNEL_M1 0
-#define PWM_CHANNEL_M2 1
\ No newline at end of file
+#define PWM_CHANNEL_M2 1
+#define UPDATE_TIME 25
+#define SPEED_STEPS 5       //  A total of 20 levels ( 100 / SPEED_STEPS ) * UPDATE_TIME = 500ms
+#define PWM_MIN 40
+#define PWM_MAX 245         // Max 98% of 2^PWM_RES
\ No newline at end of file
diff --git a/src/motorControl.h b/src/motorControl.h
index e69de29..372aa89 100644
--- a/src/motorControl.h
+++ b/src/motorControl.h
@@ -0,0 +1,30 @@
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+
+#include <cstdint>
+
+class MotorControl {
+    public:
+        MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
+        void runMotorControl();
+        void setTargetSpeed(int16_t speed);
+        void stop();
+        void emergencyStop();
+
+    private:
+        void setSpeed(int16_t speed);
+        void accelerate(int8_t acc);
+
+        int16_t target_speed = 0;
+        int16_t speed = 0;
+        uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
+
+        uint64_t last_millis = 0;
+
+        uint8_t pwm_pin;
+        uint8_t pwm_channel;
+        uint8_t direction_pin_1;
+        uint8_t direction_pin_2;
+};
+
+#endif // MOTOR_CONTROL_H
\ No newline at end of file
diff --git a/src/mototControl.cpp b/src/mototControl.cpp
index e69de29..2775651 100644
--- a/src/mototControl.cpp
+++ b/src/mototControl.cpp
@@ -0,0 +1,135 @@
+#include "motorControl.h"
+
+#include <Arduino.h>
+
+#include "config.h"
+
+MotorControl::MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
+    this->pwm_pin = pwm_pin;
+    this->pwm_channel = pwm_channel;
+    this->direction_pin_1 = direction_pin_1;
+    this->direction_pin_2 = direction_pin_2;
+
+    pinMode(this->direction_pin_1, OUTPUT);
+    pinMode(this->direction_pin_2, OUTPUT);
+
+    digitalWrite(this->direction_pin_1, LOW);
+    digitalWrite(this->direction_pin_2, LOW);
+
+    ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
+    ledcAttachPin(this->pwm_pin, this->pwm_channel);
+    ledcWrite(this->pwm_channel, 0);
+}
+
+void MotorControl::runMotorControl() {
+    uint64_t now_millis = millis();
+    if (now_millis - this->last_millis > UPDATE_TIME) {
+
+        // Absolute difference between target_speed and speed
+        uint8_t abs_difference = abs(this->target_speed - this->speed);
+
+        // Difference between target_speed and speed
+        int16_t difference = this->target_speed - this->speed;
+
+        // Check that the target speed is close to 0 and that the abs_difference is lower than SPEED_STEPS
+        if (abs(this->target_speed) <  SPEED_STEPS && abs_difference < SPEED_STEPS) {
+            this->setSpeed(0);
+            return;
+        }
+
+        // Correct speed
+        if (abs_difference < SPEED_STEPS) {
+            return;
+        }
+
+        // Positive or negative tagret speed
+        if (this->target_speed >= 0) {
+
+            // Positive or negative speed
+            if (this->speed >= 0) {
+                if (difference > 0) {
+                    this->accelerate(SPEED_STEPS);
+                } else {
+                    this->accelerate(-SPEED_STEPS);
+                }
+            } else {
+                this->accelerate(SPEED_STEPS);
+            }
+
+        } else {
+
+            // Positive or negative speed
+            if (this->speed >= 0) {
+                this->accelerate(-SPEED_STEPS);
+            } else {
+                if (difference > 0) {
+                    this->accelerate(SPEED_STEPS);
+                } else {
+                    this->accelerate(-SPEED_STEPS);
+                }
+            }
+        }
+    }
+
+    this->last_millis = millis();
+}
+
+void MotorControl::setTargetSpeed(uint8_t speed) {
+    //TODO: Exceptionhandling
+    if (speed <= 100 || speed <= -100) {
+        this->target_speed = speed;
+    } else {
+        Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
+    }
+}
+
+void MotorControl::stop() {
+    this->target_speed = 0;
+}
+
+void MotorControl::emergencyStop() {
+    setSpeed(0);
+}
+
+void MotorControl::setSpeed(int16_t speed) {
+    //TODO: Exceptionhandling
+    if (speed <= 100 || speed <= -100) {
+        this->target_speed = speed;
+    } else {
+        Serial.println("Invalid Argument in MotorControl::setSpeed");
+    }
+
+    if (speed == 0) {
+        this->direction = 0;
+        digitalWrite(this->direction_pin_1, LOW);
+        digitalWrite(this->direction_pin_2, LOW);
+        ledcWrite(this->pwm_channel, 0);
+        this->speed = speed;
+        return;
+    }
+
+    uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);
+
+    if (this->direction == 1 && speed < 0){  // new direction backward
+        this->direction = 2;
+        digitalWrite(this->direction_pin_1, LOW);
+        digitalWrite(this->direction_pin_2, HIGH);
+    } else if (this->direction == 2 && speed > 0){  // new direction forward
+        this->direction = 1;
+        digitalWrite(this->direction_pin_1, HIGH);
+        digitalWrite(this->direction_pin_2, LOW);
+    }
+
+    ledcWrite(this->pwm_channel, pwm_val);
+    this->speed = speed;
+}
+
+void MotorControl::accelerate(int8_t acc) {
+    //TODO: Exceptionhandling
+    if (abs(acc) > 2 * SPEED_STEPS) {
+        Serial.println("Invalid Argument in MotorControl::accelerate");
+        return;
+    }
+
+    this->setSpeed(speed + acc);
+}
\ No newline at end of file