main.cpp

00001 #include "mbed.h"
00002 #include "PwmOut.h"
00003 
00004 Timer freq;
00005 unsigned int edge1=0;
00006 unsigned short floorat = 0;
00007 unsigned short edge = 2;
00008 bool onFloor = false;
00009 PwmOut servo1(p22);
00010 PwmOut servo2(p23);
00011 Serial pc(USBTX, USBRX); // tx, rx
00012 PwmOut DCenable(p21);
00013 
00014 void edgecnter() {
00015     edge--;
00016     if(edge > 0) {
00017         freq.start();
00018         freq.reset();
00019     } else {
00020         int diff = freq.read_us();
00021     //    pc.printf("edgediff: %d\r\n",diff);
00022 
00023         if(diff < 10100 && diff> 9900)
00024             floorat = 1;
00025         else if (diff < 4050 && diff > 3950)
00026             floorat = 2;
00027         else if (diff < 2100 && diff > 1900)
00028             floorat = 3;
00029         else if (diff < 1500 && diff > 1300)
00030             floorat = 4;
00031         else if (diff < 1100 && diff > 900)
00032             floorat = 5;
00033         edge = 2;
00034  //       if(floorat==0)
00035  //           pc.printf("edgediff: %d\r\n",diff);
00036         freq.stop();
00037     }
00038 }
00039 
00040 void open_door()
00041 {
00042     servo1 = 0.03f; // right
00043     servo2 = 0.10f; // left
00044     wait(1);
00045 }
00046 
00047 void close_door()
00048 {
00049     servo1 = 0.08f; //right
00050     servo2 = 0.05f; //left
00051     wait(1);
00052 }
00053 
00054 short get_requested(float voltage){
00055  //   pc.printf("voltage: %f\r\n",voltage);
00056     if(voltage < 0.30f)
00057         return 1;
00058     else if (voltage < 0.55f)
00059         return 2;
00060     else if (voltage < 0.75f)
00061         return 3;
00062     else if (voltage < 0.9f)
00063         return 4;
00064     else
00065         return 5;
00066 }
00067 
00068 int main() {
00069 
00070 
00071     InterruptIn floor(p17);
00072     AnalogIn request(p18);
00073     servo1.period(0.020f);       //set 20ms period
00074     servo2.period(0.020f);       //set 20ms period
00075     DigitalOut motor1(p19);
00076     DigitalOut motor2(p20);
00077     //int floorat;
00078     DCenable.period(0.02f);
00079     
00080     
00081     floor.rise(&edgecnter);
00082 
00083 //    while(1) { 
00084 //        pc.printf("floor: %d\r\n",floorat);
00085 //    }
00086     close_door();
00087     //pc.print
00088     //go up until you find a floor
00089     motor1 = 0;
00090     motor2 = 1;
00091     DCenable = 0.5f;
00092     
00093 
00094     short current_floor = floorat;
00095     pc.printf("at floor: %d\r\n",current_floor);
00096     pc.printf("go to floor: %d\r\n",get_requested(request));
00097     while(1) {
00098         while(get_requested(request) != floorat) {
00099             if(floorat!= 0)
00100               current_floor = floorat;
00101             pc.printf("at floor: %d\r\n",floorat);
00102             pc.printf("\t\tgo to floor: %d\r\n",get_requested(request));
00103             if(get_requested(request) > current_floor) {
00104                 pc.printf("go up\r\n");
00105                 motor1 = 0;
00106                 motor2 = 1;
00107                 DCenable = 0.5f;
00108             } else if(get_requested(request) < current_floor) {
00109                 pc.printf("go down\r\n");
00110                 motor1 = 1;
00111                 motor2 = 0;
00112                 DCenable = 0.5f;
00113             } else {
00114                 motor1=0;
00115                 motor2=0;
00116                 DCenable = 0;
00117             }
00118             
00119         }
00120         pc.printf("at requested floor\r\n");
00121         DCenable = 0;
00122         open_door();
00123         wait(5);
00124         close_door();
00125         while(get_requested(request) == floorat);
00126 
00127     }
00128         
00129 
00130 
00131     
00132     return 0;
00133 }