Diff: main.cpp

Revision:

0:3009a71333b6

Child:

1:c37400704a2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 01 23:45:11 2015 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "PwmOut.h"
+
+unsigned long freqcnt=0;
+int floorat = 1;
+
+#define OPENL 0.00375f
+#define OPENR 0.1125f
+
+void freq_counter() {
+    freqcnt++;
+}
+
+void timerfunc() {
+    if (freqcnt != 0) {
+        if (freqcnt <= 1)
+            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;
+    }    
+    freqcnt = 0;   
+} 
+
+void 
+
+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);
+    AnalogIn floor2(p17);
+    
+    Timeout check_floor;
+    check_floor.attach(&timerfunc,0.01f);
+    floor.rise(&freq_counter);
+    
+    while(1){
+        if(request <= 0.66f) { //first floor
+            if(floorat > 1) { //if we're above the first floor
+                //set DC motor to go down
+                motor1 = 1;
+                motor2 = 0;
+                DCenable = 0.5f;
+            }
+        } else if (request <= 1.32f) { // second floor
+            if (floorat > 2) {
+                //set DC motor to go down
+                motor1 = 1;
+                motor2 = 0;
+                DCenable = 0.5f;
+            } else if (floorat < 2) {
+                //set DC motor to go up
+                motor1 = 0;
+                motor2 = 1;
+                DCenable = 0.5f;
+            }
+        } else if (request <= 1.98f) { // third floor
+            if (floorat > 3) {
+                //set DC motor to go down
+                motor1 = 1;
+                motor2 = 0;
+                DCenable = 0.5f;
+            } else if (floorat < 3) {
+                //set DC motor to go up
+                motor1 = 0;
+                motor2 = 1;
+                DCenable = 0.5f;
+            }
+        } else if (request <= 2.64f) { // fourth floor
+            if (floorat > 4) {
+                //set DC motor to go down
+                motor1 = 1;
+                motor2 = 0;
+                DCenable = 0.5f;
+            } else if (floorat < 4) {
+                //set DC motor to go up
+                motor1 = 0;
+                motor2 = 1;
+                DCenable = 0.5f;
+            }
+        } else { //fifth floor
+            if (floorat < 5) {
+                //set DC motor to go up
+                motor1 = 0;
+                motor2 = 1;
+                DCenable = 0.5f;
+            }
+        }   //end floor checks
+        while (floorat == 0) {    //wait endlessly until we're on a floor
+        }
+        
+        
+        
+    }
+
+    
+    return 0;
+}