Diff: main.cpp

Revision:

5:8e2b7caedd1d

Parent:

4:a5a7611a8d25

Child:

6:652229327102

--- a/main.cpp	Thu Oct 15 03:50:43 2015 +0000
+++ b/main.cpp	Thu Oct 15 05:30:53 2015 +0000
@@ -1,43 +1,40 @@
 #include "mbed.h"
 #include "PwmOut.h"
 
-unsigned int freqcnt=0;
+Timer freq;
+unsigned int edge1=0;
 unsigned short floorat = 0;
+unsigned short edge = 2;
 bool onFloor = false;
 PwmOut servo1(p22);
 PwmOut servo2(p23);
 Serial pc(USBTX, USBRX); // tx, rx
 PwmOut DCenable(p21);
 
-void freq_counter() {
-    freqcnt++;
-    //DCenable = 0;
-}
+void edgecnter() {
+    edge--;
+    if(edge > 0)
+        freq.start();
+    else {
+        int diff = freq.read_us();
+ //       pc.printf("edgediff: %d\r\n",diff);
 
-void timerfunc() {  // what it does
-    if(freqcnt >10) {
-        onFloor = true;
-        //pc.printf("on floor\r\n");
+        if(diff < 10050 && diff> 9900)
+            floorat = 1;
+        else if (diff < 4050 && diff > 3950)
+            floorat = 2;
+        else if (diff < 2050 && diff > 1950)
+            floorat = 3;
+        else if (diff < 1500 && diff > 1300)
+            floorat = 4;
+        else if (diff < 1100 && diff > 900)
+            floorat = 5;
+        edge = 2;
+ //       if(floorat==0)
+ //           pc.printf("edgediff: %d\r\n",diff);
+        freq.reset();
     }
-    else
-        onFloor = false;
-//    if(DCenable.read() > 0.0f && freqcnt< 200) {
-//        if (freqcnt>90 && freqcnt<110) {    //fifth floor
-//            floorat = 5;
-//        } else if (freqcnt>60) {    //fourth floor
-//            floorat = 4;
-//        } else if (freqcnt>40) {    //third floor
-//            floorat = 3;
-//        } else if (freqcnt>20) {    //second floor
-//            floorat = 2;
-//        } else if(freqcnt > 0) { //at first floor
-//            floorat = 1;
-//        } 
-//        pc.printf("floor: %d\r\n",floorat);
-//    }
-//    pc.printf("freqcnt: %d\r\n",freqcnt);
-//    freqcnt = 0;   
-} 
+}
 
 void open_door()
 {
@@ -54,47 +51,17 @@
 }
 
 short get_requested(float voltage){
-    if(voltage < 0.25f)
+    if(voltage < 0.3f)
         return 1;
-    else if (voltage < 0.45f)
+    else if (voltage < 0.5f)
         return 2;
-    else if (voltage < 0.65f)
+    else if (voltage < 0.7f)
         return 3;
-    else if (voltage < 0.85f)
+    else if (voltage < 0.7f)
         return 4;
     else
         return 5;
 }
-  
-short get_floor() {
-    DCenable = 0;
-    wait(2);
-    float avg=0;
-    short floorat=0;
-    freqcnt=0;
-    wait(0.1f);
-    avg+=freqcnt;
-    freqcnt=0;
-    wait(0.1f);
-    avg+=freqcnt;
-    freqcnt=0;
-    wait(0.1f);
-    avg+=freqcnt;
-    pc.printf("%f\r\n",avg);
-    if (avg>270 && avg < 310) {    //fifth floor
-        floorat = 5;
-    } else if (avg>180) {    //fourth floor
-        floorat = 4;
-    } else if (avg>120) {    //third floor
-        floorat = 3;
-    } else if (avg>60) {    //second floor
-        floorat = 2;
-    } else if(avg > 0) { //at first floor
-        floorat = 1;
-    }  
-
-    return floorat;
-}  
 
 int main() {
 
@@ -109,196 +76,50 @@
     DCenable.period(0.0002f);
     
     
-    
-    Ticker check_floor; //?
-    check_floor.attach(&timerfunc,0.1f);
-    floor.fall(&freq_counter);
-    
-   close_door();
-   //pc.print
+    floor.rise(&edgecnter);
 
-    while(!onFloor) {    //go to the first floor below us
-        motor1 = 1;
-        motor2 = 0;
-        DCenable = 0.8f;
-    }
-    wait(0.5f);
-    DCenable = 0;
-    short current_floor;
-    pc.printf("at floor: %d\r\n",get_floor());
+    
+    close_door();
+    //pc.print
+    //go up until you find a floor
+    motor1 = 0;
+    motor2 = 1;
+    DCenable = 0.8f;
+    
+    while(floorat==0);  
+
+    short current_floor = floorat;
+    pc.printf("at floor: %d\r\n",current_floor);
     pc.printf("go to floor: %d\r\n",get_requested(request));
     while(1) {
-        if(get_floor() !=0)
-            current_floor = get_floor();
-    if(get_requested(request) > current_floor)
-        {
-            pc.printf("go up\r\n");
-            motor1 = 0;
-            motor2 = 1;
-            DCenable = 0.8f;
-        } else if(get_requested(request) < current_floor) {
-            pc.printf("go down\r\n");
-            motor1 = 1;
-            motor2 = 0;
-            DCenable = 0.8f;
-        } else {
-            motor1=0;
-            motor2=0;
-            DCenable = 0;
+        while(get_requested(request) != floorat) {
+            pc.printf("at floor: %d\r\n",current_floor);
+            pc.printf("go to floor: %d\r\n",get_requested(request));
+            if(get_requested(request) > current_floor) {
+                pc.printf("go up\r\n");
+                motor1 = 0;
+                motor2 = 1;
+                DCenable = 0.8f;
+            } else if(get_requested(request) < current_floor) {
+                pc.printf("go down\r\n");
+                motor1 = 1;
+                motor2 = 0;
+                DCenable = 0.8f;
+            } else {
+                motor1=0;
+                motor2=0;
+                DCenable = 0;
+
+            }
         }
-    
-    while(onFloor) {
-        wait(1);
-        if(get_floor()!=0)
-            current_floor = get_floor();
-        pc.printf("at floor: %d\r\n",current_floor);
-        DCenable = 0.8f;
-        if(get_requested(request) > current_floor)
-        {
-            pc.printf("go up\r\n");
-            motor1 = 0;
-            motor2 = 1;
-            DCenable = 0.8f;
-        } else if(get_requested(request) < current_floor) {
-            pc.printf("go down\r\n");
-            motor1 = 1;
-            motor2 = 0;
-            DCenable = 0.8f;
-        } else {
-            motor1=0;
-            motor2=0;
-            DCenable = 0;
-        }
-    }
-    pc.printf("off current floor\r\n");
-    
-    while(!onFloor) {    //go to the first floor below us
-        DCenable = 0.8f;
-    }
-    wait(0.5f);
-    DCenable = 0;
-    
-    wait(2);
-    
-    pc.printf("at floor: %d\r\n",current_floor);
-    pc.printf("go to floor: %d\r\n",get_requested(request));
-    
+        pc.printf("at requested floor\r\n");
+        open_door();
+        wait(5);
+        close_door();
+        while(get_requested(request) == floorat);
+
     }
         
- //   floorat = get_floor();
-    
-    //pc.printf("floor %d\r\n",floorat);
-    
-    
-    wait(5);
-    open_door();
-    close_door();
-    
-  
-//    while(1){
-//        pc.printf("%d floor\n\r",floorat);
-//        pc.printf("%f requested\n\r",request.read());
-//        if(request <= 0.25f) { //first floor
-//            pc.printf("go to the first floor\r\n");
-//            if(floorat > 1) { //if we're above the first floor
-//                //set DC motor to go down
-//                motor1 = 1;
-//                motor2 = 0;
-//                DCenable = 0.8f;
-//
-//            } else if (floorat == 1) {
-//                DCenable = 0;
-//                open_door();
-//                wait(10);
-//                close_door();
-//            }
-//            
-//        } else if (request <= 0.41f) { // second floor
-//            pc.printf("go to the second floor\r\n");
-//            if (floorat > 2) {
-//                //set DC motor to go down
-//                motor1 = 1;
-//                motor2 = 0;
-//                DCenable = 0.8f;
-// 
-//            } else if (floorat < 2) {
-//                //set DC motor to go up
-//                motor1 = 0;
-//                motor2 = 1;
-//                DCenable = 0.8f;
-//
-//            } else {
-//                DCenable = 0;
-//                open_door();
-//                wait(10);
-//                close_door();
-//            }
-//        } else if (request <= 0.62f) { // third floor
-//            pc.printf("go to the third floor\r\n");
-//            if (floorat > 3) {
-//                //set DC motor to go down
-//                motor1 = 1;
-//                motor2 = 0;
-//                DCenable = 0.8f;
-//    
-//            } else if (floorat < 3) {
-//                //set DC motor to go up
-//                motor1 = 0;
-//                motor2 = 1;
-//                DCenable = 0.8f;
-//            } else {
-//                DCenable = 0;
-//                open_door();
-//                wait(10);
-//                close_door();
-//            }
-//        } else if (request <= 0.85f) { // fourth floor
-//            pc.printf("go to the fourth floor\r\n");
-//            if (floorat > 4) {
-//                //set DC motor to go down
-//                motor1 = 1;
-//                motor2 = 0;
-//                DCenable = 0.8f;
-//            } else if (floorat < 4) {
-//                //set DC motor to go up
-//                motor1 = 0;
-//                motor2 = 1;
-//                DCenable = 0.8f;
-//          
-//            } else {
-//                DCenable = 0;
-//                open_door();
-//                wait(10);
-//                close_door();
-//            }
-//        } else { //fifth floor
-//            pc.printf("go to the fifth floor\r\n");
-//            if (floorat < 5) {
-//                //set DC motor to go up
-//                motor1 = 0;
-//                motor2 = 1;
-//                DCenable = 0.8f;
-//            } else {
-//                DCenable = 0;
-//                open_door();
-//                wait(10);
-//                close_door();
-//            }
-//        }   //end floor checks
-//        
-//        wait(1);
-//        if(freqcnt > 120){
-//            DCenable = 0;
-//            wait(2);
-//        }
-        //while(1) {
-//            //DCenable = 0;
-//            wait(5);
-//            pc.printf("floor %d\r\n",floorat);
-//        } //wait til we get to the next floor
-        
-        
-    //}