From 1819ce015e62909eb37463791c8e85e81db6ccc1 Mon Sep 17 00:00:00 2001
From: Mjolnir <thomas.pietrzak@inria.fr>
Date: Thu, 3 Sep 2015 14:30:03 +0200
Subject: [PATCH] fix keyb rotation + keyb calibration

---
 ArduinoKeyboard/motorcontrol.ino | 30 +++++++++++++++++-------------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/ArduinoKeyboard/motorcontrol.ino b/ArduinoKeyboard/motorcontrol.ino
index d82ecdc..bced2d6 100644
--- a/ArduinoKeyboard/motorcontrol.ino
+++ b/ArduinoKeyboard/motorcontrol.ino
@@ -1,12 +1,13 @@
 #include <Stepper.h>
 
 //DC motors
-const int motorspeed         =  33; // mm/s
-const int motorrotationspeed =  60; // deg/s
+const int motorspeed         =  15; // mm/s
+const int motorrotationspeed =  10; // deg/s
 
 //Stepper motor
-const int steps = 400;
-Stepper stepper = Stepper(steps, stepper1, stepper2, stepper3, stepper4);
+const float stepangle = 0.9;
+const int angularratio = 6;
+Stepper stepper = Stepper(360 / stepangle, stepper1, stepper2, stepper3, stepper4);
 
 void init_motors()
 {
@@ -23,6 +24,7 @@ void init_motors()
   pinMode(stepper3, OUTPUT);
   pinMode(stepper4, OUTPUT);
   stepper.setSpeed(20);
+  stepper.step(1);
 }
 
 /*
@@ -70,7 +72,9 @@ void rotate(int angle)
   
   digitalWrite(motorsenable, LOW);
   
-  stepper.step(-angle * steps / 360);
+  unsigned long steps = angle * angularratio / stepangle;
+  Serial.println(steps);
+  stepper.step(-steps);
 }
 
 /*
@@ -85,20 +89,20 @@ void translate(int distance)
   if (distance > 0)
   {
     //left motor forward
-    digitalWrite(leftmotor1, HIGH);
-    digitalWrite(leftmotor2, LOW);
+    digitalWrite(leftmotor1, LOW);
+    digitalWrite(leftmotor2, HIGH);
     //right motor forward
-    digitalWrite(rightmotor1, HIGH);
-    digitalWrite(rightmotor2, LOW);
+    digitalWrite(rightmotor1, LOW);
+    digitalWrite(rightmotor2, HIGH);
   }
   else
   {
     //left motor backward
-    digitalWrite(leftmotor1, LOW);
-    digitalWrite(leftmotor2, HIGH);
+    digitalWrite(leftmotor1, HIGH);
+    digitalWrite(leftmotor2, LOW);
     //right motor backward
-    digitalWrite(rightmotor1, LOW);
-    digitalWrite(rightmotor2, HIGH);
+    digitalWrite(rightmotor1, HIGH);
+    digitalWrite(rightmotor2, LOW);
     
     distance = -distance;
   }
-- 
2.30.2