diff --git a/stm32f103c8t6/build_number.txt b/stm32f103c8t6/build_number.txt
index cb1a40d..bbb81cf 100644
--- a/stm32f103c8t6/build_number.txt
+++ b/stm32f103c8t6/build_number.txt
@@ -1 +1 @@
-250
+279
diff --git a/stm32f103c8t6/src/pwm_output.c b/stm32f103c8t6/src/pwm_output.c
index f6b8be0..b33e15a 100644
--- a/stm32f103c8t6/src/pwm_output.c
+++ b/stm32f103c8t6/src/pwm_output.c
@@ -21,9 +21,11 @@ typedef struct
     unsigned int y;
 } Output_Coordinate_t;
 
-static Output_Coordinate_t Output_CurrentPoint = {0, 0};
-static Output_Coordinate_t Output_TargetPoint = {0, 0};
-static int Output_CurrentVelocity = 5;
+static Output_Coordinate_t Output_CurrentLineStart = {0, 0};
+static Output_Coordinate_t Output_CurrentLineEnd = {0, 0};
+static unsigned int Output_CurrentLineLength;
+static unsigned int Output_CurrentLinePosition;
+static unsigned int Output_CurrentVelocity = OUTPUT_LENGTH_SCALE / 2;
 
 bool Output_EnqueueMovement(HPGL_Movement_t movement)
 {
@@ -67,6 +69,28 @@ static void Output_PenUp(void)
     Output_PenIsDown = false;
 }
 
+static unsigned int Output_ApproximateLength(int dx, int dy)
+{
+    if(dx < 0)
+    {
+        dx = -dx;
+    }
+    if(dy < 0)
+    {
+        dy = -dy;
+    }
+
+    // Approximate vector length with alpha max plus beta min algorithm
+    if(dx < dy)
+    {
+        return (dy + dx) * OUTPUT_LENGTH_SCALE / 4;
+    }
+    else
+    {
+        return (dx + dy) * OUTPUT_LENGTH_SCALE / 4;
+    }
+}
+
 static bool Output_FetchNextPoint(void)
 {
     if(Output_BufferRead == Output_BufferWrite)
@@ -89,9 +113,13 @@ static bool Output_FetchNextPoint(void)
         return false;
     }
 
-    Output_TargetPoint.x = movement->x;
-    Output_TargetPoint.y = movement->y;
-    // TODO: Handle pen state
+    Output_CurrentLineStart = Output_CurrentLineEnd;
+    Output_CurrentLineEnd.x = movement->x;
+    Output_CurrentLineEnd.y = movement->y;
+    Output_CurrentLineLength = Output_ApproximateLength(
+        Output_CurrentLineEnd.x - Output_CurrentLineStart.x,
+        Output_CurrentLineEnd.y - Output_CurrentLineStart.y);
+    Output_CurrentLinePosition = 0;
 
     Output_BufferRead++;
     if(Output_BufferRead >= Output_Buffer + CONFIG_BUFFER_MOVEMENTS)
@@ -101,32 +129,10 @@ static bool Output_FetchNextPoint(void)
     return true;
 }
 
-static int Output_ApproximateLength(int dx, int dy)
+static void Output_Position(Output_Coordinate_t position)
 {
-    if(dx < 0)
-    {
-        dx = -dx;
-    }
-    if(dy < 0)
-    {
-        dy = -dy;
-    }
-
-    // Approximate vector length with alpha max plus beta min algorithm
-    if(dx < dy)
-    {
-        return dy + dx / 4;
-    }
-    else
-    {
-        return dx + dy / 4;
-    }
-}
-
-static void Output_Position(void)
-{
-    uint16_t compare_x = OUTPUT_PWM_OFFSET + Output_CurrentPoint.x;
-    uint16_t compare_y = OUTPUT_PWM_OFFSET + Output_CurrentPoint.y;
+    uint16_t compare_x = OUTPUT_PWM_OFFSET + position.x;
+    uint16_t compare_y = OUTPUT_PWM_OFFSET + position.y;
 
     TIM1->CCR1 = compare_x;
     TIM1->CCR2 = compare_y;
@@ -140,32 +146,42 @@ void Output_Tick(void)
         return;
     }
 
-    int step_length = Output_CurrentVelocity;
+    unsigned int step_length = Output_CurrentVelocity;
+    Output_Coordinate_t position;
     while(true)
     {
-        // Compute distance to current target point
-        int diff_x = (int)(Output_TargetPoint.x) - (int)(Output_CurrentPoint.x);
-        int diff_y = (int)(Output_TargetPoint.y) - (int)(Output_CurrentPoint.y);
-        int length_ahead = Output_ApproximateLength(diff_x, diff_y);
+        // Compute distance from current position to current line’s end
+        unsigned int length_ahead = Output_CurrentLineLength
+            - Output_CurrentLinePosition;
         if(step_length <= length_ahead)
         {
-            // Move closer to current target point
-            Output_CurrentPoint.x += diff_x * step_length / length_ahead;
-            Output_CurrentPoint.y += diff_y * step_length / length_ahead;
+            // Move closer to current line end point
+            Output_CurrentLinePosition += step_length;
+            unsigned int remaining = Output_CurrentLineLength
+                - Output_CurrentLinePosition;
+            position.x = (Output_CurrentLineEnd.x * Output_CurrentLinePosition
+                    + Output_CurrentLineStart.x * remaining)
+                / Output_CurrentLineLength;
+            position.y = (Output_CurrentLineEnd.y * Output_CurrentLinePosition
+                    + Output_CurrentLineStart.y * remaining)
+                / Output_CurrentLineLength;
             break;
         }
 
         // Current target point is not far enough away to keep velocity
         step_length -= length_ahead;
+        position = Output_CurrentLineEnd;
+        Output_CurrentLinePosition = Output_CurrentLineLength;
+
+        // Replace line start with current line end and fetch new line end
         if(!Output_FetchNextPoint())
         {
-            // No more points in queue, stop here
-            Output_CurrentPoint = Output_TargetPoint;
+            // No more points in queue or pen state change
             break;
         }
     }
 
-    Output_Position();
+    Output_Position(position);
 }
 
 void Output_Init(void)
diff --git a/stm32f103c8t6/src/pwm_output.h b/stm32f103c8t6/src/pwm_output.h
index d2ca20c..19cfc9e 100644
--- a/stm32f103c8t6/src/pwm_output.h
+++ b/stm32f103c8t6/src/pwm_output.h
@@ -9,5 +9,10 @@
 #define OUTPUT_RESOLUTION       14  // In bits
 #define OUTPUT_COORDINATE_LIMIT ((1 << OUTPUT_RESOLUTION) - 1)
 
+// Scale for length and velocity values to allow speeds slower than 1 axis
+// resolution bit per tick. Adjust carefully as values too high can lead to
+// overflows during coordinate calculations.
+#define OUTPUT_LENGTH_SCALE     (1 << 3)
+
 void Output_Init(void);
 bool Output_EnqueueMovement(HPGL_Movement_t movement);