From 5b3adbf65dc4cb185bb620d48aa472ee9e26db39 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Sat, 25 Aug 2018 19:51:22 +0200
Subject: [PATCH] Stepper driver draft

---
 Makefile                |  1 +
 src/drivers/stepper.cpp | 86 +++++++++++++++++++++++++++++++++++++++++
 src/drivers/stepper.h   | 45 +++++++++++++++++++++
 3 files changed, 132 insertions(+)
 create mode 100644 src/drivers/stepper.cpp
 create mode 100644 src/drivers/stepper.h

diff --git a/Makefile b/Makefile
index b562dc7..0b80079 100644
--- a/Makefile
+++ b/Makefile
@@ -367,6 +367,7 @@ OBJECTS += ./src/test.o
 #OBJECTS += ./src/drivers/HMC5883L.o
 OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
+OBJECTS += ./src/drivers/stepper.o
 
 
 INCLUDE_PATHS += -I../
diff --git a/src/drivers/stepper.cpp b/src/drivers/stepper.cpp
new file mode 100644
index 0000000..138cea5
--- /dev/null
+++ b/src/drivers/stepper.cpp
@@ -0,0 +1,86 @@
+#include "drivers/stepper.h"
+
+namespace drivers {
+
+Stepper::Stepper(PinName stepPin, 
+                 PinName dirPin, 
+                 PinName enPin)
+: m_step(stepPin)
+, m_dir(dirPin)
+, m_en(enPin)
+, m_stepsPerRevolution(200)
+, m_microStepResolution(8)
+, m_currentPeriod()
+{
+    m_step.pulsewidth_us(1);
+
+    m_en.write(1);
+}
+
+void Stepper::init()
+{
+
+}
+
+void Stepper::enable()
+{
+    // Controller is enabled when IO is low
+    m_en.write(0);
+}
+
+void Stepper::disable()
+{
+    // Controller is disabled when IO is high
+    m_en.write(1);
+}
+
+bool Stepper::isEnabled()
+{
+    // Controller is enabled when IO is low
+    return m_en.read() == 0;
+}
+
+void Stepper::setDirection(int dir)
+{
+    m_dir.write(dir);
+}
+
+int Stepper::getDirection()
+{
+    return m_dir.read();
+}
+
+void Stepper::setSpeed(const double& DPS)
+{
+    if (DPS == 0)
+    {
+        disable();
+        return;
+    }
+    else
+    {
+        enable();
+    }
+
+    double revPerSecond = abs(DPS)/360.0;
+
+    double stepsPerSecond = 
+        m_stepsPerRevolution*m_microStepResolution*revPerSecond;
+
+    double usPerSecond = 1000000;
+
+    double periodUs = usPerSecond/stepsPerSecond;
+    
+    if (periodUs == m_currentPeriod)
+    {
+        return;
+    }
+    m_currentPeriod = periodUs;
+
+    m_step.period_us(periodUs);
+    m_step.pulsewidth_us(5);
+
+    m_dir = (DPS > 0) ? 1 : 0;
+}
+
+} // namespace drivers
\ No newline at end of file
diff --git a/src/drivers/stepper.h b/src/drivers/stepper.h
new file mode 100644
index 0000000..83a8236
--- /dev/null
+++ b/src/drivers/stepper.h
@@ -0,0 +1,45 @@
+
+#pragma once
+
+#include "mbed.h"
+
+namespace drivers {
+
+class Stepper
+{
+public:
+
+    Stepper(PinName stepPin, 
+            PinName dirPin, 
+            PinName enPin);
+
+    void init();
+
+    void enable();
+
+    void disable();
+
+    bool isEnabled();
+
+    void setDirection(int dir);
+
+    int getDirection();
+
+    // Steps per second? / deg per second?
+    void setSpeed(const double& DPS);
+
+private:
+
+    PwmOut m_step;
+    
+    DigitalOut m_dir, m_en;
+
+    int m_stepsPerRevolution;
+
+    int m_microStepResolution;
+
+    int m_currentPeriod;
+    
+};
+
+} // namespace drivers