main.cpp

00001 // Initialize a pins to perform Serial Communication for receive
00002 // the result of the printf on PC (USB Virtual Com)
00003 // I suggest to use TeraTerm on PC.
00004 // TeraTerm configuration must be: 9600-8-N-1 FlowControl None
00005 
00006 /* EXAMPLE */
00007 #include "mbed.h"
00008 #include "hcsr04.h"
00009 
00010 Serial pc(USBTX, USBRX);
00011 
00012 
00013 DigitalOut linksachter(D9);
00014 DigitalOut rechtsachter(A6);
00015 DigitalOut rechtsvoor(D7);
00016 DigitalOut linksvoor(D10);
00017 PwmOut pwm(D6); // links
00018 PwmOut pwm2(D11);
00019 
00020 HCSR04 sensor(D4, A0);
00021 
00022 
00023 
00024 DigitalIn lijnsensor4(D1);
00025 DigitalIn lijnsensor3(D0);
00026 DigitalIn lijnsensor1(D2);
00027 DigitalIn lijnsensor2(D3);
00028 
00029 
00030 HCSR04 sensor2(D4, A2);
00031 //HCSR04 sensor2(D4, A0);
00032 //HCSR04 sensor3(D4, A0);
00033 //HCSR04 sensor4(D4, A0);
00034 
00035 
00036 
00037 Timer timer;
00038 Timer timer2;
00039 Timer timer3;
00040 Timer timer4;
00041 Timer timer5;
00042 Timer timer6;
00043 
00044 
00045 enum ROBOTSTATE {vooruit, lijn_detect_linksvoor, lijn_detect_rechtsvoor, lijn_detect_linksachter, lijn_detect_rechtsachter, lijn_detect_voor, lijn_detect_achter, robot_detect_voor, robot_detect_rechts, robot_detect_links, robot_detect_achter, start,};
00046 ROBOTSTATE myState;
00047 int main()
00048 {
00049      pwm.period_ms(10/6);
00050     pwm.pulsewidth_ms(1);
00051     pwm2.period_ms(10/6);
00052     pwm2.pulsewidth_ms(1);
00053 
00054     while(1) {
00055         
00056           
00057 
00058        switch(myState) {
00059 
00060   
00061 
00062     
00063 
00064             case vooruit:
00065               pwm.period_ms(10/6);
00066     pwm.pulsewidth_ms(1);
00067     pwm2.period_ms(10/6);
00068     pwm2.pulsewidth_ms(1);
00069             
00070                 pwm=0.4;
00071                 pwm2=0.4;
00072                 linksvoor=1;
00073                 rechtsvoor=1;
00074                 linksachter=0;
00075                 rechtsachter=0;
00076                 if(lijnsensor1==0) {
00077                     myState = lijn_detect_linksvoor;
00078                 }
00079                 if(lijnsensor2==0) {
00080                     myState=lijn_detect_rechtsvoor;
00081                 }
00082                 if (lijnsensor3 ==0) {
00083                     myState=lijn_detect_linksachter;
00084                 }
00085                 if (lijnsensor4==0) {
00086                     myState = lijn_detect_rechtsachter;
00087                 }
00088                 if (sensor.distance()<50) { myState = robot_detect_voor; }
00089                 if (sensor2.distance()<30) {myState = robot_detect_achter;}
00090            break;
00091                
00092              
00093 
00094             case lijn_detect_linksvoor:
00095   
00096                 linksvoor=1;
00097                 rechtsvoor=0;
00098                 linksachter=0;
00099                 rechtsachter=0;
00100                 pwm=0.9;
00101                 pwm2=0.9;
00102                 wait_ms(2000);
00103                 linksvoor=1;
00104                 rechtsvoor=1;
00105                 wait_ms(500);
00106                 myState = vooruit;
00107                 break;
00108 
00109             case lijn_detect_rechtsvoor:
00110      
00111 
00112                 linksvoor=0;
00113                 rechtsvoor=1;
00114                 linksachter=0;
00115                 rechtsachter=0;
00116                 pwm=0.9;
00117                 pwm2=0.9;
00118                 wait_ms(2000);
00119                 linksvoor=1;
00120                 rechtsvoor=1;
00121                 wait_ms(500);
00122                 myState= vooruit;
00123                 break;
00124 
00125 
00126             case lijn_detect_linksachter:
00127    
00128                 timer3.start();
00129                 linksvoor=0;
00130                 rechtsvoor=1;
00131                 linksachter=0;
00132                 rechtsachter=0;
00133                 pwm=0.7;
00134                 pwm2=0.8;
00135                 if(timer3.read_ms() >= 500 ) {
00136                     timer3.stop();
00137                     timer3.reset();
00138                     myState = vooruit;
00139                 }
00140                 break;
00141 
00142             case lijn_detect_rechtsachter:
00143     
00144                 timer4.start();
00145                 linksvoor=1;
00146                 rechtsvoor=0;
00147                 linksachter=0;
00148                 rechtsachter=0;
00149                 pwm=0.7;
00150                 pwm2=0.8;
00151                 if(timer4.read_ms() >= 500 ) {   // voor twee seconden draait hij
00152                     timer4.stop();
00153                     timer4.reset();
00154                     myState = vooruit;
00155                 }
00156                 break;
00157 
00158         
00159             case robot_detect_voor:
00160  
00161               
00162             timer5.start();
00163             linksvoor=1;
00164             rechtsvoor=1;
00165             linksachter=0;
00166             rechtsachter=0;
00167             pwm=1;
00168             pwm2=1;
00169             
00170              if(lijnsensor1==0) {
00171                     timer5.stop(); timer5.reset();  myState = lijn_detect_linksvoor;
00172                 }
00173                 if(lijnsensor2==0) {
00174                    timer5.stop(); timer5.reset();  myState=lijn_detect_rechtsvoor;
00175                 }
00176                 if (lijnsensor3 ==0) {
00177                    timer5.stop(); timer5.reset();  myState=lijn_detect_linksachter;
00178                 }
00179                 if (lijnsensor4==0) {
00180                     timer5.stop(); timer5.reset();  myState = lijn_detect_rechtsachter;
00181                 }
00182             if (timer5.read_ms() >= 2000) { timer5.stop(); timer5.reset(); myState = vooruit; }
00183             break;
00184 
00185     case robot_detect_achter:
00186 
00187 
00188          timer6.start();
00189          pwm=1;
00190          pwm2=1;
00191             linksvoor=1;
00192             rechtsvoor=0;
00193            linksachter=0;
00194            rechtsachter=0;
00195            
00196              if(lijnsensor1==0) {
00197                     timer6.stop(); timer6.reset();  myState = lijn_detect_linksvoor;
00198               }
00199               if(lijnsensor2==0) {
00200                  timer6.stop(); timer6.reset();  myState=lijn_detect_rechtsvoor;
00201               }
00202              if (lijnsensor3 ==0) {
00203                   timer6.stop(); timer6.reset();  myState=lijn_detect_linksachter;
00204                }
00205                if (lijnsensor4==0) {
00206                    timer6.stop(); timer6.reset();  myState = lijn_detect_rechtsachter;
00207                 }
00208             if (timer6.read_ms() >= 1000) { timer6.stop(); timer6.reset(); myState = vooruit; }
00209    
00210            
00211 
00212 
00213 
00214 
00215 
00216 
00217         }
00218     }
00219 }