From 27710c7346bb3ac610306c084a89491aa9eff81c Mon Sep 17 00:00:00 2001
From: Mjolnir <thomas.pietrzak@inria.fr>
Date: Mon, 14 Sep 2015 11:18:14 +0200
Subject: [PATCH] slow down screen rotation

---
 ArduinoScreen/ArduinoScreen.ino |  8 ++++++++
 ArduinoScreen/motorcontrol.ino  | 24 ++++++++++++++++++++----
 2 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/ArduinoScreen/ArduinoScreen.ino b/ArduinoScreen/ArduinoScreen.ino
index a5e4e97..88d3c97 100644
--- a/ArduinoScreen/ArduinoScreen.ino
+++ b/ArduinoScreen/ArduinoScreen.ino
@@ -11,6 +11,7 @@ unsigned int motorspeed      =  77; // mm/s
 
 //Servo motor
 const int servopin      = 7;
+int servodelay = 20;
 
 //buffer stuff
 char command = 0;
@@ -60,6 +61,13 @@ void eval(int command, int value)
       Serial.print("Width: ");
       Serial.println(rackwidth);
       break;
+    case 'L':
+      servodelay = value;
+      break;
+    case 'D':
+      Serial.print("Servo delay: ");
+      Serial.println(servodelay);
+      break;
     default:
       //Serial.print("Unknown command ");
       //Serial.write(command);
diff --git a/ArduinoScreen/motorcontrol.ino b/ArduinoScreen/motorcontrol.ino
index 12ee1a9..3db5f10 100644
--- a/ArduinoScreen/motorcontrol.ino
+++ b/ArduinoScreen/motorcontrol.ino
@@ -1,5 +1,6 @@
 #include <Servo.h>
 
+
 //Servo
 Servo servo;
 
@@ -145,12 +146,27 @@ int angle()
 }
 
 /*
-  angle 
-    +: clockwise
-    -: anticlockwise
+  angle 0-180°
 */
 void rotate(int angle)
 {
-  servo.write(angle);
+  int current = servo.read();
+  
+  if(current > angle)
+  {
+    for(int pos = current ; pos > angle ; pos--)
+    {
+      servo.write(pos);
+      delay(servodelay);
+    }
+  }
+  else
+  {
+    for(int pos = current ; pos <= angle; pos++)
+    {
+      servo.write(pos);
+      delay(servodelay);
+    }
+  }
 }
 
-- 
2.30.2