diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index edf05d5..b9b064e 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -42,9 +42,15 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) {
 
 float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
 
-#include <Arduino.h>
-
 void ServoMotor::Update(float currentTimeMs) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing *child = this->GetChild(childIx);
+    if (child != nullptr && child->type == Thing::ServoType) {
+      ServoMotor *servo = (ServoMotor *)child;
+      servo->Update(currentTimeMs);
+    }
+  }
+
   if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
     this->lastUpdateTimeMs = currentTimeMs;
     return;
@@ -52,6 +58,7 @@ void ServoMotor::Update(float currentTimeMs) {
 
   float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
 
+  // Position control
   if (controlMode == ControlMode::Position) {
     if (maxVelocity == 0.0F || hasTargetAngle == false) {
       this->lastUpdateTimeMs = currentTimeMs;
@@ -77,7 +84,8 @@ void ServoMotor::Update(float currentTimeMs) {
       return;
     }
 
-  } else { // Velocity Control
+    // Velocity control
+  } else {
     float newAngle = this->targetAngle + targetVelocity * deltaTime;
     newAngle = Float::Clamp(newAngle, minAngle, maxAngle);
 
@@ -86,13 +94,4 @@ void ServoMotor::Update(float currentTimeMs) {
 
     this->lastUpdateTimeMs = currentTimeMs;
   }
-
-  Serial.println(this->childCount);
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    Thing *child = this->GetChild(childIx);
-    if (child->type == Thing::ServoType) {
-      ServoMotor *servo = (ServoMotor *)child;
-      servo->Update(currentTimeMs);
-    }
-  }
 }
\ No newline at end of file
diff --git a/VectorAlgebra b/VectorAlgebra
index 7810691..596e85d 160000
--- a/VectorAlgebra
+++ b/VectorAlgebra
@@ -1 +1 @@
-Subproject commit 78106912a9e80c1ace5533615cab0bd4a00657a0
+Subproject commit 596e85d3a41f5f25b6185f2ca4fb18a2140afca1