diff --git a/Config.h b/Config.h
index 9f878ad..30aca62 100644
--- a/Config.h
+++ b/Config.h
@@ -8,8 +8,7 @@ const byte AUTH_UID[4] = { 145, 136, 97, 102 };
 const byte AUTH_UID_SIZE = sizeof(AUTH_UID);
 
 // // Odległościomierz
-// const int TRIG_PIN = 6;
-// const int ECHO_PIN = 5;
+const int TRIG_AND_ECHO_PIN = 3;
 
 const long SONAR_MAX_DIST_CM = 400;
 
@@ -30,7 +29,12 @@ const uint8_t BRAKE_PIN = A2;
 const uint8_t THROTTLE_PIN = A3;
 
 // Silniki
+// Prawy silnik
 const int IN_1 = 9;
 const int IN_2 = 8;
+const int ENA_1 = 5;
+
+// Lewy silnik
 const int IN_3 = 4;
-const int IN_4 = 3;
+const int IN_4 = 2;
+const int ENA_2 = 6;
diff --git a/Engine.cpp b/Engine.cpp
index 5e09640..f7114ab 100644
--- a/Engine.cpp
+++ b/Engine.cpp
@@ -1,4 +1,5 @@
 #include "Engine.h"
+#include "Config.h"
 using namespace EngineConsts;
 
 Engine::Engine(uint8_t throttlePin,
@@ -6,82 +7,41 @@ Engine::Engine(uint8_t throttlePin,
                uint8_t brakePin)
   : _throttlePin(throttlePin), _clutchPin(clutchPin), _brakePin(brakePin) {}
 
-void Engine::begin() {
-}
+void Engine::begin() {}
 
 float Engine::readNormalized(uint8_t pin) const {
   return analogRead(pin) / 1023.0f;
 }
 
 void Engine::update(float dt, uint8_t gearIdx) {
-  // Serial.println("----------------");
-
   const float throttle = readNormalized(_throttlePin);
-  // Serial.print("Throttle: ");
-  // Serial.println(throttle);
-
   const float clutch = readNormalized(_clutchPin);
-  // Serial.print("Clutch: ");
-  // Serial.println(clutch);
-
   const float brake = readNormalized(_brakePin);
-  // Serial.print("Brake: ");
-  // Serial.println(brake);
 
   const float targetRPM = RPM_IDLE + throttle * (RPM_MAX - RPM_IDLE);
-  // Serial.print("targetRPM: ");
-  // Serial.println(targetRPM);
 
   const float wheelRPM = (_speedMs / (2.0f * PI * WHEEL_RADIUS)) * 60.0f;
-  // Serial.print("wheelRPM: ");
-  // Serial.println(wheelRPM);
 
   const float gearRatio = GEAR_RATIO[gearIdx];
-  // Serial.print("gearRatio: ");
-  // Serial.println(gearRatio);
-
   const float drivenRPM = wheelRPM * fabs(gearRatio) * FINAL_DRIVE;
-  // Serial.print("drivenRPM: ");
-  // Serial.println(drivenRPM);
 
   _engineRPM = clutch * drivenRPM + (1.0f - clutch) * targetRPM;
-  // Serial.print("_engineRPM: ");
-  // Serial.println(_engineRPM);
-
   _engineRPM = constrain(_engineRPM, RPM_IDLE, 7500.0f);
-  // Serial.print("_engineRPM (constrain): ");
-  // Serial.println(_engineRPM);
 
   const float engineForce = (_engineRPM / RPM_MAX) * (1.0f - clutch) * ENGINE_FORCE_FACTOR;
-  // Serial.print("engineForce: ");
-  // Serial.println(engineForce);
-
   const float brakeForce = brake * BRAKE_FORCE_MAX;
-  // Serial.print("brakeForce: ");
-  // Serial.println(brakeForce);
-
   const float dragForce = 0.5f * DRAG_COEFF * FRONTAL_AREA * (_speedMs * _speedMs);
-  // Serial.print("dragForce: ");
-  // Serial.println(dragForce);
-
   const int direction = (gearIdx == -1) ? -1 : 1;
-  // Serial.print("direction: ");
-  // Serial.println(direction);
-
   const float netForce = direction * engineForce - brakeForce - dragForce;
-  // Serial.print("netForce: ");
-  // Serial.println(netForce);
-
   const float acceleration = netForce / CAR_MASS;
-  // Serial.print("acceleration: ");
-  // Serial.println(acceleration);
 
   _speedMs += acceleration * dt;
   if (_speedMs < 0.0f) _speedMs = 0.0f;
-  // Serial.print("_speedMs: ");
-  // Serial.println(_speedMs);
 
-  // delay(1000);
+  setMotor(IN_3, IN_4, ENA_2, targetRPM, false);
+
+  bool reverseWheel = (gearIdx == -1);
+  setMotor(IN_1, IN_2, ENA_1, wheelRPM * 10.0f, reverseWheel);
 }
 
 int Engine::getRPM() const {
@@ -91,3 +51,16 @@ int Engine::getRPM() const {
 int Engine::getSpeedKmh() const {
   return static_cast<int>((_speedMs * 3.6f) + 0.5f);
 }
+
+uint8_t Engine::rpmToPwm(float rpm) {
+  rpm = constrain(rpm, 0.0f, RPM_MAX);
+  return static_cast<uint8_t>(roundf(rpm / RPM_MAX * 255.0f));
+}
+
+void Engine::setMotor(uint8_t inA, uint8_t inB, uint8_t ena,
+                      float rpm, bool reverse) {
+  digitalWrite(inA, reverse ? LOW : HIGH);
+  digitalWrite(inB, reverse ? HIGH : LOW);
+
+  analogWrite(ena, rpmToPwm(rpm));
+}
diff --git a/Engine.h b/Engine.h
index 3567b1b..9c1c07a 100644
--- a/Engine.h
+++ b/Engine.h
@@ -36,6 +36,9 @@ public:
   int getRPM() const;
   int getSpeedKmh() const;
 
+  uint8_t rpmToPwm(float rpm);
+  void setMotor(uint8_t inA, uint8_t inB, uint8_t ena, float rpm, bool reverse = false);
+
 private:
   const uint8_t _throttlePin;
   const uint8_t _clutchPin;
diff --git a/Projekt_Arduino.ino b/Projekt_Arduino.ino
index 588042c..cb7d696 100644
--- a/Projekt_Arduino.ino
+++ b/Projekt_Arduino.ino
@@ -22,6 +22,10 @@ void setup() {
 
   pinMode(IN_1, OUTPUT);
   pinMode(IN_2, OUTPUT);
+  pinMode(ENA_1, OUTPUT);
+  pinMode(IN_3, OUTPUT);
+  pinMode(IN_4, OUTPUT);
+  pinMode(ENA_2, OUTPUT);
 
   rfid.begin();
   display.begin(LCD_COLS, LCD_ROWS);
@@ -38,7 +42,7 @@ void loop() {
     carEnabled = !carEnabled;
     Serial.print("Auto działa: ");
     Serial.println(carEnabled);
-    display.startOrEnd(carEnabled);0
+    display.startOrEnd(carEnabled);
   };
 
   if (carEnabled) {