Lab3

Dependencies:   mbed-rtos mbed

Fork of ESE519_Lab3_EC_v4 by ese519

Revision:
1:e79ac0826624
Parent:
0:015087d61ca1
Child:
2:d7103c7e0671
--- a/main.cpp	Sat Oct 03 00:02:31 2015 +0000
+++ b/main.cpp	Fri Oct 16 16:41:38 2015 +0000
@@ -1,147 +1,270 @@
+// ESE 519 Lab 3 Code //
+
 #include "mbed.h"
 
 Serial pc(USBTX,USBRX);
+
 // functions
-void find_keys();
-Timer t;
+//void find_keys();
+void check_floor();
+void get_floor();   // uses floor period to set cur_floor
+void get_period();
+void led_update();
+
+// Threads
+void bpc_func(void const *args);
+Thread * bpc_thread;
 
 // init board
 PwmOut dc_motor(p26);
 PwmOut servo1(p25);
 PwmOut servo2(p24);
-DigitalOut in1(p14);    // elevator direction
-DigitalOut in2(p13);    // elevator direction
-AnalogIn IRSensor(p16);      // read IR sensor
-//AnalogOut test(p18);
+DigitalOut in1(p11);        // elevator direction
+DigitalOut in2(p12);        // elevator direction
+InterruptIn IRSensor(p10);  // read IR sensor
+AnalogIn Button(p15);       // elevator button press Voltage = 3.3*0.2*n ; n = Floor Number
 
 // keyboard read
+/*
 DigitalIn col1(p20);
 DigitalIn col2(p19);
 DigitalIn col3(p18);
 DigitalOut row1(p30);
 DigitalOut row2(p27);
+*/
 
-DigitalOut ind1(LED1);
-DigitalOut ind2(LED2);
-DigitalOut ind3(LED3);
-DigitalOut ind4(LED4);
-
+// LEDs for testing
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
 // init vars
-float ADC_Voltage;
+Timer t;
 float period = 0.02;    // sec
 float dty_dc = 0.75;    // PCT [0-100]
 float dty_servo_open = 0.1125;  // PCT [3.75-11.25]
 float dty_servo_close = 0.0375; // PCT [3.75-11.25]
-int cur_floor = 1;
-int desired_floor = 1;
-int tperiod;
+int cur_floor = 0;
+int desired_floor = 0;
+int floor_period;   // for period detection
+int keyPressed = 0;
+int count = 0;
+int test_readings = 0;  // need 4 consecutive readings to declare cur_floor
+int test_floor = 0;
 
 int main() {
-    
+   
+    // start button checker thread
+    bpc_thread = new Thread(bpc_func);
+   
+    // init interrupt handler
+    IRSensor.fall(&get_period);
+   
     // set period (constant)
     dc_motor.period(period);
-   // servo1.period(period);
-   // servo2.period(period);
-    
+    servo1.period(period);
+    servo2.period(period);
+
+    // start with elevator stationary
+    dc_motor.write(0);
+
     // start with closed doors
-   // servo1.write(dty_servo_close);
-    //servo2.write(dty_servo_close);
+    servo1.write(dty_servo_close);
+    servo2.write(dty_servo_close);
 
-    // run elevator program
     while(1) {
-        
-        
+       
+        // get init floor
+        while (!cur_floor) { // wait for a floor to be detected
+            wait(0.25);
+        }
+        desired_floor = cur_floor;
+        led_update();
+       
         // check for key press
-        find_keys();
-        
-        // check if need to move
-        if (cur_floor != desired_floor) {
-            
-            // determine direction
-            if (cur_floor > desired_floor) {    // move down
-                in1 = 0;
-                in2 = 1;
-            }
-            else {  // move up
-                in1 = 1;
-                in2 = 0;
+        //find_keys();
+
+        // execute elevator alg
+        if (keyPressed) {
+
+            // check if need to move
+            if (cur_floor != desired_floor) {
+
+                // determine direction
+                if (cur_floor > desired_floor) {    // move down
+                    in1 = 1;
+                    in2 = 0;
+                } else {                            // move up
+                    in1 = 0;
+                    in2 = 1;
+                }
+
+                // start car
+                dc_motor.write(dty_dc);
+
+                // check IR sensors
+                while (cur_floor != desired_floor) {
+                    wait(0.2);
+                }
+
+                // stop car
+                dc_motor.write(0);
             }
-            
-            // start car
-            dc_motor.write(dty_dc);
-            
-            // check IR sensors
-            
-            while (cur_floor != desired_floor) {
-                // check IR emitter here!
-                
-                while(!ADC_Voltage)
-                    ADC_Voltage = IRSensor.read();
-            
-                t.start();
-            
-                while(ADC_Voltage)
-                    ADC_Voltage = IRSensor.read();
-           
-                t.stop();
-        
-                tperiod = 2 * t.read_us();
-        
-                t.reset();
-        
-            if(tperiod > 9900 && tperiod < 10100)
-                cur_floor = 1;
-            else if(tperiod > 3900 && tperiod < 4100)
-                cur_floor = 2;
-            else if(tperiod > 1900 && tperiod < 2100)
-                cur_floor = 3;
-            else if(tperiod > 1300 && tperiod < 1500)
-                cur_floor = 4;
-            else if(tperiod > 900 && tperiod < 1100)
-                cur_floor = 5;
-                
-            //pc.printf("%d", cur_floor);
-            //wait_ms(1000);
-            }
+
+            // open door
+            servo1.write(dty_servo_open);
+            servo2.write(dty_servo_open);
+            keyPressed = 0;
         }
-            
-            // stop car 
-            dc_motor.write(0);
-                      
-      //  }
-        
-        // open door
-        
-      //  servo1.write(dty_servo_open);
-       // servo2.write(dty_servo_open);
-        
-         
-    } 
-    
-     
+    }
+
 }
 
+/*
 void find_keys() {
-    
+
     // check row 1
     row1 = 0;
     row2 = 1;
     if (!col1) {
-        desired_floor = 1;    
+        desired_floor = 1;
+        keyPressed = 1;
     }
     if (!col2) {
-        desired_floor = 2;    
+        desired_floor = 2;
+        keyPressed = 1;
     }
     if (!col3) {
-        desired_floor = 3;    
+        desired_floor = 3;
+        keyPressed = 1;
     }
-    
+   
     // check row 2
     row1 = 1;
     row2 = 0;
     if (!col1) {
         desired_floor = 4;
+        keyPressed = 1;
     }
-    
+    if (!col2) {
+        desired_floor = 5;
+        keyPressed = 1;
+    }
+}
+*/
+
+void get_floor() {
+    if(floor_period > 9900 && floor_period < 10100) {
+        if (test_floor == 1) { 
+            test_readings++;
+        } else {
+            test_readings = 0;
+        }
+        test_floor = 1;
+        if (test_readings > 3) cur_floor = 1;
+    } else if(floor_period > 3900 && floor_period < 4100) {
+        if (test_floor == 2) { 
+            test_readings++;
+        } else {
+            test_readings = 0;
+        }    
+        test_floor = 2;
+        if (test_readings > 3) cur_floor = 2;
+    } else if(floor_period > 1900 && floor_period < 2100) {
+        if (test_floor == 3) { 
+            test_readings++;
+        } else {
+            test_readings = 0;
+        }
+        test_floor = 3;
+        if (test_readings > 3) cur_floor = 3;
+    } else if(floor_period > 1300 && floor_period < 1500) {
+        if (test_floor == 4) { 
+            test_readings++;
+        } else {
+            test_readings = 0;
+        }
+        test_floor = 4;
+        if (test_readings > 3) cur_floor = 4;
+    } else if(floor_period > 900 && floor_period < 1100) {
+        if (test_floor == 5) { 
+            test_readings++;
+        } else {
+            test_readings = 0;
+        }
+        test_floor = 5;
+        if (test_readings > 3) cur_floor = 5;
+    }   
 }
+
+void get_period() {     
+    count++;
+    if (count == 1)
+        t.start();
+    else if (count == 2) {
+        t.stop();
+        floor_period = t.read_us();
+        t.reset();
+        get_floor();
+        count = 0;
+    }
+}
+
+void led_update() {
+    if (cur_floor == 1) {
+        led1 = 1;
+        led2 = 0;
+        led3 = 0;
+        led4 = 0;
+    }
+    if (cur_floor == 2) {
+        led2 = 1;
+        led1 = 0;
+        led3 = 0;
+        led4 = 0;
+    }
+    if (cur_floor == 3) {
+        led3 = 1;
+        led2 = 0;
+        led1 = 0;
+        led4 = 0;
+    }
+    if (cur_floor == 4) {
+        led4 = 1;
+        led2 = 0;
+        led3 = 0;
+        led1 = 0;
+    }
+    if (cur_floor == 5) {
+        led4 = 1;
+        led1 = 1;
+        led2 = 0;
+        led3 = 0;
+    }
+}
+
+void bpc_func(void const *args) {
+    while (1) {
+        float ADC_val = Button.read(); 
+        int val = ADC_val*10;
+        if(val == 2) {
+            desired_floor = 1;
+            keyPressed = 1;
+        } else if(val == 4) {
+            desired_floor = 2;
+            keyPressed = 1;
+        } else if(val == 6) {
+            desired_floor = 3;
+            keyPressed = 1;
+        } else if(val == 8) {
+            desired_floor = 4;
+            keyPressed = 1;
+        } else if(val == 10) {
+            desired_floor = 5;
+            keyPressed = 1;
+        }
+        wait(0.1);
+        while (keyPressed); // wait for elevator to arrive at desired floor
+    }
+}
\ No newline at end of file