Screen calibration

diff --git a/ArduinoScreen/ArduinoScreen.ino b/ArduinoScreen/ArduinoScreen.ino
index 13c4f3524575e139e4ae421b72fb2c855474c998..15901a63a3466f3e0cfab248b08c487e2b4048ac 100644 (file)
--- a/ArduinoScreen/ArduinoScreen.ino
+++ b/ArduinoScreen/ArduinoScreen.ino
@@ -6,6 +6,8 @@ const int motorpin1     = 5;
 const int motorpin2     = 4;
 const int motorsenable  = 6;
 
+unsigned int motorspeed =  77; // mm/s
+
 //Servo motor
 const int servopin     = 7;
 
@@ -35,6 +37,17 @@ void eval(int command, int value)
       Serial.println(value);
       rotate(value);
       break;
+    case 'C':
+      Serial.println("Calibration");
+      calibrate();
+      break;
+    case 'S':
+      Serial.print("Motor speed: ");
+      Serial.println(motorspeed);
+      break;
+    default:
+      Serial.print("Unknown command ");
+      Serial.println(command);
   }
 }
 
@@ -43,6 +56,9 @@ void setup()
 {
   //Motors
   init_motors();
+  
+  //Calibration
+  calibrate();
 
   //Communication
   Serial.begin(9600);
@@ -59,11 +75,13 @@ void loop()
     
     if (c == '\n' || c == '\r')
     {
+      //if we have a command => evaluate it
       if (command)
       {
         if (sign == NEGATIVE)
           value = -value;
         eval(command, value);
+        //reset command
         command = 0;
         value = 0;
         sign = POSITIVE;

diff --git a/ArduinoScreen/motorcontrol.ino b/ArduinoScreen/motorcontrol.ino
index 7b86200c62fd75ddd2f5edac2486ad8d23a9793f..674687c76d982430daf0de57fe5d58d5667feb20 100644 (file)
--- a/ArduinoScreen/motorcontrol.ino
+++ b/ArduinoScreen/motorcontrol.ino
@@ -1,7 +1,7 @@
 #include <Servo.h>
 
 //DC motors
-const int motorspeed         =  33; // mm/s
+const unsigned int rackwidth = 644; //mm
 
 //Servo
 Servo servo;
@@ -22,6 +22,46 @@ void init_motors()
   servo.attach(servopin);
 }
 
+void calibrate()
+{
+  digitalWrite(motorsenable, HIGH);
+  //move to left
+  digitalWrite(motorpin1, LOW);
+  digitalWrite(motorpin2, HIGH);
+  while(digitalRead(limitleftpin))
+    delayMicroseconds(10);
+  digitalWrite(motorpin1, LOW);
+  digitalWrite(motorpin2, LOW);
+  
+  unsigned long start = millis();
+  
+  //move to right
+  digitalWrite(motorpin1, HIGH);
+  digitalWrite(motorpin2, LOW);
+  while(digitalRead(limitrightpin))
+    delayMicroseconds(10);
+  digitalWrite(motorpin1, LOW);
+  digitalWrite(motorpin2, LOW);
+  
+  unsigned long delta = millis() - start;
+
+  motorspeed = (1000. * rackwidth) / delta;
+  
+  start = millis();
+  unsigned long duration = delta / 2;
+  
+  //move to center
+  digitalWrite(motorpin1, LOW);
+  digitalWrite(motorpin2, HIGH);
+  while(millis() - start < duration)
+    delayMicroseconds(10);
+  digitalWrite(motorpin1, LOW);
+  digitalWrite(motorpin2, LOW);
+  
+  digitalWrite(motorsenable, LOW);
+}
+
+
 /*
   distance 
     +: right
@@ -29,49 +69,48 @@ void init_motors()
 */
 void translate(int distance)
 {
-  unsigned int end = 0;
+  unsigned long end = 0;
   digitalWrite(motorsenable, HIGH);
   
-  //forward
+  //right
   if (distance > 0)
   {
     digitalWrite(motorpin1, HIGH);
     digitalWrite(motorpin2, LOW);
     end = millis() + (distance * 1000.) / motorspeed;
   }
+  //left
   else
   {
-    //left motor backward
     digitalWrite(motorpin1, LOW);
     digitalWrite(motorpin2, HIGH);
-    
     end = millis() + (-distance * 1000.) / motorspeed;
   }
   
   while(millis() < end)
   {
+    //right
     if (distance > 0)
     {
       if (!digitalRead(limitrightpin))
       {
-        Serial.println("Left reached");
+        Serial.println("Left limit reached");
         break;
       }
       delayMicroseconds(10);
     }
+    //left
     else
     {
       if (!digitalRead(limitleftpin))
       {
-        Serial.println("right  reached");
+        Serial.println("Right limit reached");
         break;
       }
       delayMicroseconds(10);
     }
   }
-  
-  Serial.println("Finished");
-  
+    
   //motor stop
   digitalWrite(motorpin1, LOW);
   digitalWrite(motorpin2, LOW);