Diff: main.cpp

Revision:

4:95fe80130442

Parent:

3:c3d716d6c6a7

--- a/main.cpp	Thu Mar 26 23:41:44 2015 +0000
+++ b/main.cpp	Fri Mar 27 03:53:57 2015 +0000
@@ -7,18 +7,23 @@
 #include "RequestQueue.h"
  
 DigitalOut myled(LED1);
+InterruptIn irInput(p14);
+Timer irTimer;
 
 void moveForward (){
     dcMotor.forward();
     movingDirection = 1;
+    irFlag = 1;
 }
 
 void moveReverse(){
     dcMotor.reverse();
     movingDirection = -1;
+    irFlag = 1;
 }
 
 void floorChosen (int i){
+    pc.printf(":  %d floor is chosen  :",i);
     queue.add(i);
     if(!dcMotor.isMoving()){
         if(i == currentFloor){  
@@ -66,20 +71,67 @@
     currentFloor = i;
     if(queue.isRequested(i))
         floorReached(i);
+    else{
+        irFlag = 1;
+        if(i == 1 && movingDirection == -1){
+            dcMotor.stop();
+            if(queue.isForwardRequested(i)){
+                dcMotor.forward();
+            }
+        }else if(i==5 && movingDirection == 1){
+            dcMotor.stop();
+            if(queue.isReverseRequested(i)){
+                dcMotor.reverse();
+            }
+        }
+    }
+}
+
+int calculateFloor (float timePeriod){
+    float f = 1/timePeriod;
+    //pc.printf("In calculateFloor %f",f);
+    if (f <120.0 && f >80.0){
+        return 1;
+    }else if (f <220.0 && f >180.0){
+        return 2;
+    }else if (f <320.0 && f >280.0){
+        return 3;
+    }else if (f <420.0 && f >380.0){
+        return 4;
+    }else if (f <520.0 && f >480.0){
+        return 5;
+    }else
+        return 0;
+}
+
+
+int isFirstRise = 1;
+void irRise(){
+    //pc.printf("Interrupt detected: %d",irFlag);
+    if(irFlag){
+        if(isFirstRise){
+            irTimer.start();
+            isFirstRise = 0;
+        }else{
+            irTimer.stop();
+            currentFloor = calculateFloor(irTimer.read());
+            if(currentFloor){
+                irFlag = 0;
+                floorDetected(currentFloor);
+            }
+            irTimer.reset();    
+            isFirstRise = 1;
+        }
+    }
 }
 
 int main() {
     initializeKeypad();
+    irInput.rise(&irRise);
     while(1) {
-        //dcMotor.forward();
-        leftDoor.open();
-        rightDoor.open();
-        myled = 1;
-        wait(3);
-        //dcMotor.stop();
-        leftDoor.close();
-        rightDoor.close();
-        myled = 0;
-        wait(3);
+       myled = 1;
+       wait(1);
+       myled = 0;
+       wait(1);
     }
 }
\ No newline at end of file