diff --git a/src/drivers/stepper.cpp b/src/drivers/stepper.cpp
index a88b27f..532868a 100644
--- a/src/drivers/stepper.cpp
+++ b/src/drivers/stepper.cpp
@@ -1,4 +1,7 @@
 #include "src/drivers/stepper.h"
+#include "src/math/Utilities.h"
+
+using namespace math;
 
 namespace drivers {
 
@@ -8,13 +11,19 @@ Stepper::Stepper(PinName stepPin,
 : m_step(stepPin)
 , m_dir(dirPin)
 , m_en(enPin)
+, m_accelerationLimitOn(true)
+, m_accelerationLimit(50.0f)
 , m_stepsPerRevolution(200)
 , m_microStepResolution(8)
-, m_currentPeriod()
+, m_currentPeriod(0)
+, m_configuredDirection(1)
+, m_lastDirection(1)
+, m_latestSpeed(0.0f)
 {
     m_step.pulsewidth_us(1);
 
     m_en.write(1);
+    m_dir.write(m_configuredDirection);
 }
 
 void Stepper::init()
@@ -42,45 +51,76 @@ bool Stepper::isEnabled()
 
 void Stepper::setDirection(int dir)
 {
-    m_dir.write(dir);
+    m_configuredDirection = dir;
+    m_lastDirection = dir;
+    m_dir.write(dir == 1 ? 1 : 0);
 }
 
 int Stepper::getDirection()
 {
-    return m_dir.read();
+    return m_configuredDirection;
 }
 
-void Stepper::setSpeed(const double& DPS)
+float Stepper::limitAcceleration(float DPS)
 {
-    if (DPS == 0)
-    {
-        disable();
-        return;
-    }
-    else
-    {
-        enable();
-    }
+    float delta = DPS - m_latestSpeed;
 
-    double revPerSecond = abs(DPS)/360.0;
+    if (abs(delta) > m_accelerationLimit)
+    {
+        return (delta > 0) ? 
+            m_latestSpeed + m_accelerationLimit : 
+            m_latestSpeed - m_accelerationLimit;
+    }
+    return DPS;
+}
 
-    double stepsPerSecond = 
+void Stepper::setSpeed(const float& DPS)
+{
+    m_latestSpeed = limitAcceleration(DPS);
+
+    float revPerSecond = abs(m_latestSpeed)/(float)360.0;
+    revPerSecond = constrain(revPerSecond, 1000000.0f);
+
+    float stepsPerSecond = 
         m_stepsPerRevolution*m_microStepResolution*revPerSecond;
 
-    double usPerSecond = 1000000;
+    float usPerSecond = 1000000.0f;
 
-    double periodUs = usPerSecond/stepsPerSecond;
+    float periodUs = (stepsPerSecond == 0.0) ? 100000.0 : usPerSecond/stepsPerSecond;
     
-    if (periodUs == m_currentPeriod)
+    // Precaution. Don't know how close the period can be to 
+    // the pulsewidth or if it will cause any issues
+    if (periodUs < (float)100.0f)
     {
-        return;
+        periodUs = (float)100.0f;
+    }
+    if (periodUs > (float)100000.0f)
+    {
+        periodUs = (float)100000.0f;
+    }
+
+    // No need to set anything new
+    if (periodUs != m_currentPeriod)
+    {
+        m_step.period_us(periodUs);
+        m_step.pulsewidth_us(5.0f);
     }
     m_currentPeriod = periodUs;
 
-    m_step.period_us(periodUs);
-    m_step.pulsewidth_us(5);
+    int dir = (m_latestSpeed > (float)0.0f) ? 1 : -1;
 
-    m_dir = (DPS > 0) ? 1 : 0;
+    if (m_lastDirection != dir)
+    {
+        // Give dir to motor controller
+        m_dir.write((m_configuredDirection*dir == 1) ? 1 : 0);
+
+        m_lastDirection = dir;
+    }
+}
+
+float Stepper::getSpeed()
+{
+    return m_latestSpeed;
 }
 
 } // namespace drivers
\ No newline at end of file
diff --git a/src/drivers/stepper.h b/src/drivers/stepper.h
index 83a8236..5e64b66 100644
--- a/src/drivers/stepper.h
+++ b/src/drivers/stepper.h
@@ -21,12 +21,17 @@ public:
 
     bool isEnabled();
 
+    // Direction is -1 (backward) or 1 (forward)
     void setDirection(int dir);
 
     int getDirection();
 
+    float limitAcceleration(float DPS);
+
     // Steps per second? / deg per second?
-    void setSpeed(const double& DPS);
+    void setSpeed(const float& DPS);
+
+    float getSpeed();
 
 private:
 
@@ -34,11 +39,21 @@ private:
     
     DigitalOut m_dir, m_en;
 
+    bool m_accelerationLimitOn;
+
+    float m_accelerationLimit;
+
     int m_stepsPerRevolution;
 
     int m_microStepResolution;
 
     int m_currentPeriod;
+
+    int m_configuredDirection;
+
+    int m_lastDirection;
+
+    float m_latestSpeed;
     
 };