diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h
index 4f00c2f..2538e10 100644
--- a/lib/Chassis/src/chassis.h
+++ b/lib/Chassis/src/chassis.h
@@ -2,25 +2,10 @@
 
 #include <Arduino.h>
 #include "event_timer.h"
+#include "constants.h"
 
 class Chassis
 {
-protected:
-    /**
-     * Kinematic parameters default to the spec sheet from Pololu. You'll need to fine
-     * tune them.
-     */
-    const float ROBOT_RADIUS = 14.7 / 2;
-    const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
-    const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
-
-    /**
-     * You can change the control loop period, but you should use multiples of 4 ms to 
-     * avoid rounding errors.
-     */
-    const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
-    const float CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.;
-
 protected:
     /**
      * loopFlag is used to tell the program when to update. It is set when Timer4
diff --git a/lib/Chassis/src/constants.h b/lib/Chassis/src/constants.h
new file mode 100644
index 0000000..7d49d28
--- /dev/null
+++ b/lib/Chassis/src/constants.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <Arduino.h>
+
+/**
+ * Kinematic parameters default to the spec sheet from Pololu. You'll need to fine
+ * tune them.
+ */
+const float ROBOT_RADIUS = 14.7 / 2;
+const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
+const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
+
+/**
+ * You can change the control loop period, but you should use multiples of 4 ms to 
+ * avoid rounding errors.
+ */
+const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
+const float CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.;
diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index 2a773dd..5469c4f 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -2,6 +2,7 @@
 
 #include <Arduino.h>
 #include <FastGPIO.h>
+#include <constants.h>
 
 // define the motor pins here
 #define PWM_L 10
@@ -33,8 +34,10 @@ protected:
     // Used to keep track of the target speed, in counts / interval.
     float targetSpeed = 0;
 
+    float TRAPEZOIDAL_RAMP_RATE_CM_S = 30.;
     // maximum accelration in counts/interval
-    float TRAPEZOIDAL_RAMP_RATE = 0;
+    float TRAPEZOIDAL_RAMP_RATE = TRAPEZOIDAL_RAMP_RATE_CM_S / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S;
+    float currentSetpoint = 0;
 
     /**
      * This is the speed of the motor, in "encoder counts / encoder interval".
@@ -124,7 +127,12 @@ protected:
              * TODO: Implement integral, derivative control for the motors here!
              */
             // Calculate the error in speed
-            float error = targetSpeed - speed;
+
+            float deltaSetpoint = targetSpeed - currentSetpoint;
+            deltaSetpoint = constrain(deltaSetpoint, -TRAPEZOIDAL_RAMP_RATE, TRAPEZOIDAL_RAMP_RATE);
+            currentSetpoint += deltaSetpoint;
+
+            float error = currentSetpoint - speed;
 
             sumError += error;