Diff: main.cpp

Revision:

3:f45ab902c0ad

Parent:

2:dcd904176b27

Child:

4:857d2663c894

--- a/main.cpp	Wed Mar 25 23:10:44 2015 +0000
+++ b/main.cpp	Fri Mar 27 19:33:46 2015 +0000
@@ -16,7 +16,6 @@
 
 Serial pc(USBTX, USBRX);
 
-
 //itizilize digital I/O for keypad
 DigitalOut row1(p27);
 DigitalOut row2(p28);
@@ -30,12 +29,13 @@
 //init keypad map
 map<int, int> keypadMap;
 
-int curFloor = 1;  //initialize at 1
+int curFloor;  //initialize at 2
 int floorDesired = 1; //init floor buffer
-bool floors[5];
+bool floors[5] = {false, false, false,false,false};
 
+int open = 0;
 int count = 0;
-int nextFloor;
+int nextFloor = 1;
 
 //direction 1:up; 2: down;
 int direction = 0;
@@ -46,7 +46,7 @@
 float fourth = 2500.0;
 float fifth = 2000.0;
 
-float tol = 250.0;
+float tol = 150.0;
 
 void init(){
         /*
@@ -64,7 +64,8 @@
         keypadMap[0x12]= 2;
         keypadMap[0x14]= 3;
         keypadMap[0x21]= 4;
-        keypadMap[0x22]= 5;
+        keypadMap[0x24]= 5;
+        //keypadMap[0x24]= 0;
         }
 
 void openDoors(){
@@ -80,12 +81,12 @@
     }
 
 void up(){
-    EN = 0.2;
+    EN = 0.6;
     IN1 = 1;
     IN2 = 0.0;
     }
 void down(){
-    EN = 0.2;
+    EN = 0.6;
     IN1 = 0.0;
     IN2 = 1;   
     }
@@ -128,27 +129,32 @@
 int getNextFloor() {
         //up
         if (direction == 1){
-                for (int i = curFloor; i< 4; ++i){
-                    if(floors[i])
+                for (int i = curFloor; i< 5; ++i){
+                    if(floors[i]==true)
                         return i+1;
                     }
                 for (int i = curFloor - 2; i >=0 ; --i){
-                    if(floors[i])
+                    if(floors[i]==true)
                         return i+1;
                     }
+                return curFloor;
             }
         //down
-        if (direction == 2){
+        else if (direction == 2){
                 for (int i = curFloor - 2; i >=0 ; --i){
-                    if(floors[i])
+
+                    if(floors[i]==true)
                         return i+1;
                     }
-                for (int i = curFloor; i< 4; ++i){
-                    if(floors[i])
+                for (int i = curFloor; i< 5; ++i){
+                    if(floors[i]==true)
                         return i+1;
                     }
+                return curFloor;
             }
         else return floorDesired;
+        //else return;
+
     }
 
 int main() {
@@ -164,10 +170,6 @@
     irIN.rise(&irRise);
     irIN.fall(&irFall);
     
-    //ELEVATOR STATE
-    //int nofloor = 0;   //set nofloor state to 0
-    
-    //int Serv = 0;
     //initialize servo and dc motor
     float freq = 50;    //set 50 HZ freq for PWM and motor
     EN.period(1/freq);   
@@ -178,8 +180,22 @@
     rightDoor.period(1/freq);              
     rightDoor= 0.075;                  //itit door closed
     
+    int reach = 0;
+    
+    //go down until 1st floor detected as starting point
+    pc.printf("Starting elevator \r");
+    
+    while(curFloor != 1)
+    {
+        down();
+    }
+    
+    stop();
+    
+    
+    
     while(1) {
-        pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor);
+
  
 /////////////Below contains all code in order to detect current input from keypad////////////////////////////       
         if(prev_input == 0){
@@ -210,35 +226,59 @@
              cur_input |= 0x04;
              else cur_input = 0;    //Detect button release or no input set input to 0
             
+            if(cur_input == 0x22)
+                leds = 0x1;
+            
             //leds = keypadMap[cur_input];             //KEYPAD DEBUG
             if(cur_input != prev_input && cur_input != 0){
                     floorDesired = keypadMap[cur_input];
                     floors[floorDesired - 1] = true;
                     nextFloor = getNextFloor();
-                    //Serv++;
+                    pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
+
                 }
             //Maintain the past input
             prev_input = cur_input;
+            //fix this error i don't know why it exsits
+
             
 ////////////////////////MOVE FLOORS//////////////////////////////////////////////////   
          
-    
-        if(curFloor == nextFloor){
+        
+        if(curFloor == nextFloor){ //if correct stop moving and get the next floor
             stop();
+            if(open == 0)
+            { 
+            openDoors();
+            open = 1;
+            }
             floors[nextFloor-1] = false;
-            nextFloor = getNextFloor();
+            if(reach == 0 ){
+                
+                nextFloor = getNextFloor();
+                wait(1);
+                //pc.printf("Current Floor = %d, Next Floor = %d\r",curFloor,nextFloor);
+                reach = 1;
+                pc.printf("Current Floor = %d, Next Floor = %d, Direction = %d, Last floor input = %d\r",curFloor,nextFloor, direction, floorDesired);
+                }
+                reach = 1;
             }
-        if(curFloor < nextFloor){
+        if(curFloor < nextFloor){  //if the cur floor is below the nextfloor move up
+            reach = 0;
+            closeDoors();
             up();
             direction = 1;
             }
-        if(curFloor > nextFloor){
+        if(curFloor > nextFloor){ //if the cur floor is above the nextfloor move down
+            if(open == 1)
+            { 
+            closeDoors();
+            open = 0;
+            }
+            reach = 0;
             down();
             direction = 2;
             }
                         
-            
-            } 
-       
-    
+            }     
 }