1 files changed, 29 insertions, 0 deletions
diff --git a/Motor.cpp b/Motor.cpp
new file mode 100644
index 0000000..7060967
--- /dev/null
+++ b/Motor.cpp
@@ -0,0 +1,29 @@
+#include "Motor.h"
+
+#include "util.h"
+
+
+Motor::Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask) {
+  this->port = port;
+  this->pwmPort = pwmPort;
+  this->fwdMask = fwdMask;
+  this->revMask = revMask;
+  
+  setSpeed(0);
+}
+
+void Motor::setSpeed(int speed) {
+  this->speed = CLAMP(-255, speed, 255);
+  
+  if(speed > 0) {
+    *port &= ~revMask;
+    *port |= fwdMask;
+  }
+  else if(speed < 0) {
+    *port &= ~fwdMask;
+    *port |= revMask;
+  }
+  else *port |= fwdMask|revMask;
+  
+  *pwmPort = ABS(this->speed);
+}