diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index 7978534..633844c 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -2,20 +2,26 @@
 
 #include "VectorAlgebra/FloatSingle.h"
 
-ServoMotor::ServoMotor() { this->controlMode = ControlMode::Position; }
+ServoMotor::ServoMotor() {
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = 0;
+  this->hasTargetAngle = false;
+}
 
-void ServoMotor::SetTargetAngle(float targetAngle) {
-  targetAngle = Float::Clamp(targetAngle, minAngle, maxAngle);
+void ServoMotor::SetTargetAngle(float angle) {
+  angle = Float::Clamp(angle, minAngle, maxAngle);
 
-  if (maxVelocity == 0.0F) {
-    SetAngle(targetAngle);
-  } else if (targetAngle != this->targetAngle) {
+  if (maxVelocity == 0.0F || hasTargetAngle == false) {
+    SetAngle(angle);
+    this->limitedTargetAngle = angle;
+  } else if (angle != this->targetAngle) {
     // if the new target is the same, the limited angle is not overwritten
     this->limitedTargetAngle = this->targetAngle;
   }
 
   this->controlMode = ControlMode::Position;
-  this->targetAngle = targetAngle;
+  this->targetAngle = angle;
+  this->hasTargetAngle = true;
 }
 
 float ServoMotor::GetTargetAngle() { return this->targetAngle; }
@@ -24,7 +30,7 @@ void ServoMotor::SetMaximumVelocity(float maxVelocity) {
   this->maxVelocity = maxVelocity;
 }
 
-void ServoMotor::SetTargetVelocity(float velocity) {
+void ServoMotor::SetTargetVelocity(float targetVelocity) {
   targetVelocity =
       Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity);
 
@@ -51,27 +57,30 @@ void ServoMotor::Update(float currentTimeMs) {
     } else {
       float angleStep = maxVelocity * deltaTime;
       float deltaAngle = this->targetAngle - this->limitedTargetAngle;
-      if (deltaAngle < 0)
-        deltaAngle = -deltaAngle;
+      float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
 
-      if (deltaAngle < angleStep) {
+      if (absDeltaAngle < angleStep) {
         this->limitedTargetAngle = this->targetAngle;
         SetAngle(targetAngle);
       } else {
-        limitedTargetAngle += angleStep;
+        if (deltaAngle < 0)
+          limitedTargetAngle -= angleStep;
+        else
+          limitedTargetAngle += angleStep;
       }
       SetAngle(limitedTargetAngle);
 
       this->lastUpdateTimeMs = currentTimeMs;
       return;
     }
+
+  } else { // Velocity Control
+    float newAngle = this->targetAngle + targetVelocity * deltaTime;
+    newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
+
+    ServoMotor::SetTargetAngle(newAngle);
+    SetAngle(newAngle);
+
+    this->lastUpdateTimeMs = currentTimeMs;
   }
-
-  float newAngle = this->targetAngle + targetVelocity * deltaTime;
-  newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
-
-  ServoMotor::SetTargetAngle(newAngle);
-  SetAngle(newAngle);
-
-  this->lastUpdateTimeMs = currentTimeMs;
 }
\ No newline at end of file