Diff: main.cpp

Revision:

2:27a9092dcc6c

Parent:

1:c37400704a2f

Child:

3:4f15d12f0ac0

diff -r c37400704a2f -r 27a9092dcc6c main.cpp
--- a/main.cpp	Wed Oct 07 01:08:21 2015 +0000
+++ b/main.cpp	Fri Oct 09 22:58:27 2015 +0000
@@ -2,33 +2,27 @@
 #include "PwmOut.h"
 
 unsigned int freqcnt=0;
-unsigned int floorat = 1;
+unsigned short floorsPassed = 0;
+bool onFloor = false;
 PwmOut servo1(p22);
 PwmOut servo2(p23);
 Serial pc(USBTX, USBRX); // tx, rx
-DigitalOut DCenable(p21);
+PwmOut DCenable(p21);
 
 void freq_counter() {
     freqcnt++;
-    DCenable = 0;
-    pc.printf("%d cnt\r\n",freqcnt);
+    //DCenable = 0;
 }
 
 void timerfunc() {  // what it does
-    
-    floorat = 0;
-    if (freqcnt != 0) {
-        if (freqcnt <= 1 && freqcnt>0)
-            floorat = 1;
-        else if (freqcnt <= 3)
-            floorat = 2;
-        else if (freqcnt <= 5)
-            floorat = 3;
-        else if (freqcnt <= 7)
-            floorat = 4;
-        else if (freqcnt <= 10)
-            floorat = 5;
-    }    
+    if (freqcnt > 10) {
+        onFloor = true;
+        
+    } else {
+        if(onFloor)
+            floorsPassed++;
+        onFloor = false;
+    }
     freqcnt = 0;   
 } 
 
@@ -45,7 +39,30 @@
     servo2 = 0.05f; //left
     wait(1);
 }
-    
+  
+short get_floor(int i) {
+    int floorat=0;
+    wait(0.099f);
+    if (freqcnt>90) {    //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;
+    }  
+    if(i>0) {
+        int floor2 = get_floor(i-1);
+        if (floor2 == floorat)
+            return floorat;
+        else
+            return floor2;
+    }
+    return floorat;
+}  
 
 int main() {
 
@@ -56,51 +73,64 @@
     servo2.period(0.020f);       //set 20ms period
     DigitalOut motor1(p19);
     DigitalOut motor2(p20);
-    AnalogIn floor2(p17);
+    int floorat;
+    DCenable.period(0.0002f);
     
     
     
     Ticker check_floor; //?
-    check_floor.attach(&timerfunc,0.01f);
+    check_floor.attach(&timerfunc,0.1f);
     floor.fall(&freq_counter);
     
    close_door();
+   //pc.print
 
-   
+    while(!onFloor) {    //go to the first floor below us
+        motor1 = 1;
+        motor2 = 0;
+        DCenable = 0.8f;
+    }
+    
+    pc.printf("at floor\r\n");
+    floorat = get_floor(1);
+    
+    pc.printf("floor %d\r\n",floorat);
+    
+    DCenable = 0;
+    wait(5);
+    open_door();
+    close_door();
+    
+  
     while(1){
         pc.printf("%d floor\n\r",floorat);
         pc.printf("%f requested\n\r",request.read());
-        if(DCenable == 0 && floorat == 0) {     //if we are not starting on a known floor
-            motor1 = 1;         //just go up until you get to a floor
-            motor2 = 0;
-            DCenable = 1;
-            pc.printf("go up to start \r\n");
-        }
         if(request <= 0.2f) { //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 = 1;
+                DCenable = 0.8f;
             } else if (floorat == 1) {
                 DCenable = 0;
                 open_door();
                 wait(10);
                 close_door();
             }
+            
         } else if (request <= 0.4f) { // 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 = 1;
+                DCenable = 0.8f;
             } else if (floorat < 2) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else {
                 DCenable = 0;
                 open_door();
@@ -113,12 +143,12 @@
                 //set DC motor to go down
                 motor1 = 1;
                 motor2 = 0;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else if (floorat < 3) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else {
                 DCenable = 0;
                 open_door();
@@ -131,12 +161,12 @@
                 //set DC motor to go down
                 motor1 = 1;
                 motor2 = 0;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else if (floorat < 4) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else {
                 DCenable = 0;
                 open_door();
@@ -149,7 +179,7 @@
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 1;
+                DCenable = 0.8f;
             } else {
                 DCenable = 0;
                 open_door();
@@ -157,16 +187,10 @@
                 close_door();
             }
         }   //end floor checks
-        wait(10);
+        while(!onFloor) {} //wait til we get to the next floor
+        floorat = get_floor(1);
     }
-        //while (floorat == 0) {    //wait endlessly until we're on a floor
-//            if(DCenable == 0 || freqcnt != 0)
-//                break;
-//            wait(1);
-//        }
-    while(1)    {
-        
-    }
+
 
     
     return 0;