From 7da669b97ec71f138c74d562b8a3b219d3f98a50 Mon Sep 17 00:00:00 2001
From: neoraider <devnull@localhost>
Date: Fri, 13 Apr 2007 17:58:03 +0000
Subject: Makefile angelegt; Grundfunktionen erweitert

---
 Motor.cpp | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)
 create mode 100644 Motor.cpp

(limited to 'Motor.cpp')

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);
+}
-- 
cgit v1.2.3