elevator.cpp

00001 #include "mbed.h"
00002 #include "LinkedList.h"
00003 
00004 int serving;
00005 LinkedList<node>list;
00006 Serial pc(USBTX,USBRX);
00007 PwmOut dc_motor(p23);
00008 PwmOut servo1(p21);
00009 PwmOut servo2(p22);
00010 DigitalInOut in1(p5);
00011 DigitalInOut in2(p6);
00012 AnalogIn an(p16);
00013 InterruptIn firstfloor(p15);
00014 InterruptIn secondfloor(p8);
00015 InterruptIn thirdfloor(p9);
00016 InterruptIn fourthfloor(p10);
00017 InterruptIn fifthfloor(p11);
00018 InterruptIn sensor(p12);
00019 DigitalOut led4(LED4);
00020 DigitalOut led3(LED3); 
00021 DigitalOut led2(LED2); 
00022 DigitalOut led1(LED1); 
00023 int first = 0;
00024 Timer time1;
00025 int once = 0;
00026 float stationary = 0.075;
00027 float open = 0.085;   // counter clock-wise
00028 float closed = 0.040;    // clock-wise
00029 float open2 = 0.100;
00030 float closed2 = 0.055;
00031 int reached = 0;
00032 int location;
00033 float frequency;
00034 int requested_floor1, requested_floor2, requested_floor;
00035 int busy = 0;
00036 int currentfloor;
00037 int globedir = 2;
00038 
00039 
00040 int inqueue(int request){
00041     int i;
00042     int exist = 0;
00043     node *tmp;
00044     for (i = 1; i<= list.length();i++){
00045         tmp = list.pop(i);
00046         if (request == (int) tmp->data)
00047             exist = 1;
00048             }
00049     return exist;
00050 }
00051 
00052 int dir(int current, int next){
00053     int direction;
00054     if (current > next)
00055         direction = 0;
00056     if (current <= next)
00057         direction = 1;
00058 return direction;
00059 }
00060 void serve(){
00061         
00062     if(busy == 1){ 
00063         if (first == 0){
00064             time1.reset();
00065             time1.start();
00066             first = 1;
00067         }
00068             
00069         else if (first == 1){
00070             time1.stop();
00071             first = 0;
00072             once++;
00073             frequency = 1./time1.read();
00074 
00075             if (once == 50){
00076                 once = 0;
00077                 
00078                 if (frequency >= 900.0f && frequency <= 1100.0f){currentfloor = 5;}
00079                 else if (frequency >= 600.0f && frequency <= 800.0f){currentfloor = 4;}
00080                 else if (frequency >= 400.0f && frequency <= 590.0f){currentfloor = 3;}
00081                 else if (frequency >= 200.0f && frequency <= 300.0f){currentfloor = 2;}
00082                 else if (frequency >= 50.0f && frequency <= 150.0f){currentfloor = 1;}
00083         
00084                 if (currentfloor == serving){
00085                     pc.printf("served:   %d floor frequency:  %f \n",currentfloor,frequency);
00086             
00087                     dc_motor.write(0.0f);
00088                     led1 = 0;
00089                     led2 = 0;
00090                     led3 = 0;
00091                     led4 = 0;
00092                     in1 = 0;
00093                     in2 = 0;
00094                     busy = 0;
00095                     servo1.write(open);
00096                     servo2.write(open2);
00097                     wait(1); 
00098                     location = serving;
00099                     pc.printf("at:   %d \n",location);
00100                     sensor.fall(0);
00101                     }
00102 
00103         }
00104  
00105 }
00106 }
00107     }
00108     
00109 void moveUp(){
00110     dc_motor.write(0.5f);
00111     in1 = 1;
00112     in2 = 0;
00113     }
00114     
00115 void moveDown(){
00116     
00117     dc_motor.write(0.5f);
00118     in1 = 0;
00119     in2 = 1;
00120     }
00121 
00122 
00123 void closeDoor(){
00124     servo1.write(closed);
00125     servo2.write(closed2); 
00126 }
00127 
00128 void openDoor(){
00129     servo1.write(open);
00130     servo2.write(open2); 
00131 }
00132 
00133 void call(int next) {
00134     node *tmp;
00135     int i;
00136     int assigned = 0;
00137     
00138     if (list.length() < 4 && next > 0 && inqueue(next) == 0){
00139         list.append((int *)next);
00140         }
00141     for (i = 0; i<= list.length();i++){
00142         tmp = list.pop(i);    
00143         pc.printf(" %d \n",(int)tmp->data);
00144         }       
00145     if (busy == 0 && (int)list.pop(1)->data < 6){
00146         
00147         if (globedir == 2){
00148                     tmp = list.pop(1);
00149                     serving = (int)tmp->data;
00150                     list.remove(1);
00151                     assigned = 1;
00152                     }
00153                     
00154         else if (globedir == 1){
00155             for (i = 1; i<= list.length();i++){
00156                 tmp = list.pop(i);
00157                 if (dir(location,(int) tmp->data) == 1){
00158                     serving = (int)tmp->data;
00159                     list.remove(i);
00160                     assigned = 1;
00161                     break;
00162                 }
00163                 }
00164             }
00165             
00166         else if (globedir == 0){
00167             for (i = 1; i<= list.length();i++){
00168                 tmp = list.pop(i);
00169                 if (dir(location,(int) tmp->data) == 0){
00170                     serving = (int)tmp->data;
00171                     list.remove(i);
00172                     assigned = 1;
00173                     break;
00174             }
00175         }        
00176             }
00177         if (assigned != 1){
00178             tmp = list.pop(1);
00179             serving = (int)tmp->data;
00180             list.remove(1);
00181             }
00182             
00183             
00184         if (location > serving){
00185             pc.printf("serving:   %d \n",serving);
00186             globedir = 0;
00187             closeDoor();
00188             moveDown();
00189             busy = 1;
00190             sensor.fall(&serve);
00191             }
00192             
00193         else if (location < serving ){
00194             pc.printf("serving:   %d \n",serving);
00195             globedir = 1;
00196             closeDoor();
00197             moveUp();
00198             busy = 1;
00199             sensor.fall(&serve);
00200             }
00201             
00202     }
00203     }
00204     
00205 int main() {
00206     servo1.period(0.02f);
00207     servo2.period(0.02f);
00208     in1.output();
00209     in2.output();
00210     in2 = 0;
00211     in1 = 0; 
00212     location = 2;
00213     while(1){
00214         
00215         while(list.length() <= 4){
00216         
00217             if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor1 = 1;}      //1
00218             else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor1 = 2;}//2
00219             else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor1 = 3;} //3
00220             else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor1 = 4;}  //4
00221             else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor1 = 5;}  //5
00222             else{requested_floor1 = 0;}
00223             
00224             if (an.read()*3.3f > 0.62f && an.read()*3.3f < 1.0f ){requested_floor2 = 1;led1 = 1;}    
00225             else if (an.read()*3.3f > 1.20f && an.read()*3.3f < 1.60f ){requested_floor2 = 2;led2 = 1;} 
00226             else if (an.read()*3.3f > 1.70f && an.read()*3.3f < 2.3f ){requested_floor2 = 3;led3 = 1;} 
00227             else if (an.read()*3.3f > 2.4f && an.read()*3.3f < 2.9f ){requested_floor2 = 4; led4 = 1;} 
00228             else if (an.read()*3.3f > 3.0f && an.read()*3.3f < 3.5f ){requested_floor2 = 5;}
00229             else{requested_floor2 = 0;}
00230             
00231             if (requested_floor1 != requested_floor2) {requested_floor = 0;}
00232             else{requested_floor = requested_floor1;}
00233             if ((inqueue(requested_floor) == 0 && requested_floor != 0) || list.length() > 0){
00234                 call(requested_floor); 
00235                 }
00236 }
00237 }
00238 }