Diff: main.cpp

Revision:

1:c37400704a2f

Parent:

0:3009a71333b6

Child:

2:27a9092dcc6c

--- a/main.cpp	Thu Oct 01 23:45:11 2015 +0000
+++ b/main.cpp	Wed Oct 07 01:08:21 2015 +0000
@@ -1,19 +1,24 @@
 #include "mbed.h"
 #include "PwmOut.h"
 
-unsigned long freqcnt=0;
-int floorat = 1;
-
-#define OPENL 0.00375f
-#define OPENR 0.1125f
+unsigned int freqcnt=0;
+unsigned int floorat = 1;
+PwmOut servo1(p22);
+PwmOut servo2(p23);
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalOut DCenable(p21);
 
 void freq_counter() {
     freqcnt++;
+    DCenable = 0;
+    pc.printf("%d cnt\r\n",freqcnt);
 }
 
-void timerfunc() {
+void timerfunc() {  // what it does
+    
+    floorat = 0;
     if (freqcnt != 0) {
-        if (freqcnt <= 1)
+        if (freqcnt <= 1 && freqcnt>0)
             floorat = 1;
         else if (freqcnt <= 3)
             floorat = 2;
@@ -27,80 +32,139 @@
     freqcnt = 0;   
 } 
 
-void 
+void open_door()
+{
+    servo1 = 0.03f; // right
+    servo2 = 0.10f; // left
+    wait(1);
+}
+
+void close_door()
+{
+    servo1 = 0.08f; //right
+    servo2 = 0.05f; //left
+    wait(1);
+}
+    
 
 int main() {
-    PwmOut DCenable(p21);
-    PwmOut servo1(p22);
-    PwmOut servo2(p23);
+
+
     InterruptIn floor(p17);
     AnalogIn request(p18);
     servo1.period(0.020f);       //set 20ms period
     servo2.period(0.020f);       //set 20ms period
-    DigitalOut motor1(p20);
-    DigitalOut motor2(p19);
+    DigitalOut motor1(p19);
+    DigitalOut motor2(p20);
     AnalogIn floor2(p17);
     
-    Timeout check_floor;
+    
+    
+    Ticker check_floor; //?
     check_floor.attach(&timerfunc,0.01f);
-    floor.rise(&freq_counter);
+    floor.fall(&freq_counter);
     
+   close_door();
+
+   
     while(1){
-        if(request <= 0.66f) { //first floor
+        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 = 0.5f;
+                DCenable = 1;
+            } else if (floorat == 1) {
+                DCenable = 0;
+                open_door();
+                wait(10);
+                close_door();
             }
-        } else if (request <= 1.32f) { // second floor
+        } 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 = 0.5f;
+                DCenable = 1;
             } else if (floorat < 2) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 0.5f;
+                DCenable = 1;
+            } else {
+                DCenable = 0;
+                open_door();
+                wait(10);
+                close_door();
             }
-        } else if (request <= 1.98f) { // third floor
+        } else if (request*5 <= 0.6f) { // 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.5f;
+                DCenable = 1;
             } else if (floorat < 3) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 0.5f;
+                DCenable = 1;
+            } else {
+                DCenable = 0;
+                open_door();
+                wait(10);
+                close_door();
             }
-        } else if (request <= 2.64f) { // fourth floor
+        } else if (request <= 0.8f) { // 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.5f;
+                DCenable = 1;
             } else if (floorat < 4) {
                 //set DC motor to go up
                 motor1 = 0;
                 motor2 = 1;
-                DCenable = 0.5f;
+                DCenable = 1;
+            } 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.5f;
+                DCenable = 1;
+            } else {
+                DCenable = 0;
+                open_door();
+                wait(10);
+                close_door();
             }
         }   //end floor checks
-        while (floorat == 0) {    //wait endlessly until we're on a floor
-        }
-        
-        
+        wait(10);
+    }
+        //while (floorat == 0) {    //wait endlessly until we're on a floor
+//            if(DCenable == 0 || freqcnt != 0)
+//                break;
+//            wait(1);
+//        }
+    while(1)    {
         
     }