diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index 87c04c6..9ec5ef4 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -12,7 +12,7 @@ ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
 #include <Arduino.h>
 
 void ControlledMotor::SetTargetSpeed(float speed) {
-  this->targetSpeed = speed;
+  this->currentTargetSpeed = speed;
   // this->rotationDirection =
   //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
   // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
@@ -21,22 +21,31 @@ void ControlledMotor::SetTargetSpeed(float speed) {
 
 void ControlledMotor::Update(float currentTimeMs) {
   actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
-  if (this->targetSpeed < 0)
+  if (this->currentTargetSpeed < 0)
     actualSpeed = -actualSpeed;
 
+  float deltaTime = currentTimeMs - this->lastUpdateTime;
+
+  float error = currentTargetSpeed - actualSpeed;
+  float errorChange = (error - lastError) * deltaTime;
+
+  float delta = error * pidP + errorChange * pidD;
+
   // Serial.print("direction ");
   // Serial.print(this->direction);
-  // Serial.print("  actualSpeed ");
-  // Serial.println(actualSpeed);
+  Serial.print("  actualSpeed ");
+  Serial.print(actualSpeed);
   // Serial.print("  target ");
-  // Serial.println(this->targetSpeed);
+  // Serial.println(this->currentTargetSpeed);
+  Serial.print("    motor target speed ");
+  Serial.print(motor->currentTargetSpeed);
+  Serial.print(" + ");
+  Serial.print(error * pidP);
+  Serial.print(" + ");
+  Serial.println(errorChange * pidD);
 
-  float error = targetSpeed - velocity;
-
-  float timeStep = currentTimeMs - lastUpdateTime;
-  float acceleration = error * timeStep * pidP; // Just P is used at this moment
-  motor->SetTargetSpeed(
-      this->targetSpeed); // + acceleration); // or something like that
+  motor->SetTargetSpeed(motor->currentTargetSpeed +
+                        delta); // or something like that
   this->lastUpdateTime = currentTimeMs;
 }
 
diff --git a/ControlledMotor.h b/ControlledMotor.h
index 48bbfb4..6ddd74f 100644
--- a/ControlledMotor.h
+++ b/ControlledMotor.h
@@ -19,8 +19,8 @@ public:
   }
   float velocity;
 
-  float pidP = 1;
-  float pidD = 0;
+  float pidP = 0.02F;
+  float pidD = 0.001F;
   float pidI = 0;
 
   void Update(float currentTimeMs) override;
@@ -39,8 +39,9 @@ public:
   Encoder *encoder;
 
 protected:
-  float lastUpdateTime;
-  float targetSpeed;
+  float lastUpdateTime = 0;
+  float lastError = 0;
+  // float targetSpeed;
   float actualSpeed;
   float netDistance = 0;
   float startDistance = 0;
diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp
index 9e0c2cf..ce46f08 100644
--- a/DifferentialDrive.cpp
+++ b/DifferentialDrive.cpp
@@ -23,7 +23,8 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
   rightMotor->position.distance = distance;
 }
 
-void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
+void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
+                                             float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     Motor *motor = motors[motorIx];
     if (motor == nullptr)
@@ -40,7 +41,8 @@ void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
 void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
   float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
   float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
-  SetTargetSpeeds(leftSpeed, rightSpeed);
+  // The speeds should be translated to m/s to revolutions per second here
+  SetMotorTargetSpeeds(leftSpeed, rightSpeed);
 }
 
 void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
diff --git a/DifferentialDrive.h b/DifferentialDrive.h
index 919a044..155f73c 100644
--- a/DifferentialDrive.h
+++ b/DifferentialDrive.h
@@ -33,7 +33,7 @@ public:
   /// @brief Set the target speeds of the motors directly
   /// @param leftSpeed The target speed of the left Motor
   /// @param rightSpeed The target speed of the right Motor
-  void SetTargetSpeeds(float leftSpeed, float rightSpeed);
+  void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);
 
   /// @brief Controls the motors through forward and rotation speeds
   /// @param forward The target forward speed of the Roboid
diff --git a/Motor.h b/Motor.h
index 8890bfa..1655893 100644
--- a/Motor.h
+++ b/Motor.h
@@ -30,8 +30,9 @@ public:
 
   virtual void Update(float currentTimeMs);
 
-protected:
   float currentTargetSpeed = 0;
+
+protected:
 };
 
 } // namespace RoboidControl