Neues Rescue-Programm angefangen (2)

1 files changed, 16 insertions, 0 deletions

diff --git a/avr.c b/avr.c
new file mode 100644
index 0000000..ac303d0
--- /dev/null
+++ b/avr.c
@@ -0,0 +1,16 @@
+#include "avr.h"
+#include "util.h"
+
+void setMotorSpeed(MOTOR *motor, int speed) {
+  if(speed > 0) {
+    *motor->port &= ~motor->revMask;
+    *motor->port |= motor->fwdMask;
+  }
+  else if(speed < 0) {
+    *motor->port &= ~motor->fwdMask;
+    *motor->port |= motor->revMask;
+  }
+  else *motor->port |= motor->fwdMask|motor->revMask;
+  
+  *motor->pwmPort = CLAMP(0, ABS(speed), 255);
+}