main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "HCSR04.h"
00004 
00005 Motor mSx(D10, D8, D9);     //pwm (E1), fwd (1A), rev (1B)
00006 Motor mDx(D5, D6, D7);      //pwm (E2), fwd (2A), rev (2B)
00007 
00008 HCSR04 sensor(D11, D12);    //echo pin, trig pin
00009 
00010 AnalogIn luxSx(A2);         //PhotoR Sx
00011 AnalogIn luxFw(A1);         //PhotoR Fw
00012 AnalogIn luxDx(A0);         //PhotoR Dx
00013 
00014 //DigitalOut ledSx(D2);
00015 //DigitalOut ledFw(D3);
00016 //DigitalOut ledDx(D4);
00017 
00018 Serial pc (SERIAL_TX, SERIAL_RX);
00019 
00020 RawSerial hc05(D13, D14);
00021 
00022 Ticker time_up; 
00023 
00024 //DigitalIn button(PC_13);
00025 
00026 float inv;
00027 float vel;
00028 float delta;
00029 float dist;
00030 float soglia;
00031 char  a;
00032 
00033 void forward(float v) //v rta 0 e 1
00034 {
00035     mDx.speed(v);//DX
00036     mSx.speed(v); //SX
00037     
00038     //ledSx=0;
00039     //ledFw=1;
00040     //ledDx=0;
00041     
00042     pc.printf("Fw");
00043 }
00044 
00045 void turnDx (float v, float t) //t tra 0 e 1
00046 {
00047     mDx.speed(v*t);//DX
00048     mSx.speed(v); //SX
00049     
00050     //ledSx=0;
00051     //ledFw=0;
00052     //ledDx=1;
00053     
00054     pc.printf("Dx");
00055 }
00056 void turnSx (float v, float t)
00057 {    
00058     mDx.speed(v);//DX
00059     mSx.speed(v*t); //SX
00060     
00061     //ledSx=1;
00062     //ledFw=0;
00063     //ledDx=0;
00064     
00065     pc.printf("Sx");
00066 }
00067 
00068 void check_lux()
00069 {
00070     float Sx=luxSx.read();
00071     float Fw=luxFw.read();
00072     float Dx=luxDx.read();
00073     
00074     dist=50;           //provvisorio
00075     //dist=sensor.getDistance_cm();     //definitivo
00076     //**************
00077     
00078     if(dist>soglia)
00079     {
00080         inv=1;
00081     }
00082     else
00083     {
00084         inv=-1;
00085     }
00086     
00087     delta=Sx-Dx;
00088     
00089     if(Fw>Sx&Fw>Dx&dist>soglia)
00090     {
00091         forward(vel);
00092     }
00093     else if (delta>0)
00094     {
00095         turnSx(vel,inv*(1-delta));
00096     }
00097     else
00098     {
00099         turnDx(vel,inv*(delta-1));
00100     }
00101 }
00102 
00103 void OnOff()
00104 {
00105     if(hc05.readable())         // se e’¨ stato ricevuto un carattere 
00106     {                                       
00107         a=hc05.getc();          // assegna il carattere ricevuto alla var a
00108         switch(a)
00109         {
00110             case ('s'):         //Start
00111                 time_up.attach(&check_lux, 0.2);
00112                 pc.printf("Start %c\n\r",a);
00113                 break;
00114             case ('d'):         //Stop
00115                 time_up.detach();
00116                 pc.printf("Stop %c\n\r",a);
00117                 break;
00118             case ('a'):         //accelera
00119                 if(vel<=1)
00120                 {
00121                     vel+=0.1;
00122                 }              
00123                 break;
00124             case ('f'):         //frena
00125                 if(vel>0)
00126                 {
00127                     vel-=0.1;
00128                 }      
00129                 break;
00130             default:
00131                 break;
00132         }
00133     }
00134 }
00135 
00136 
00137 
00138 
00139 int main()
00140 {
00141     inv=1;          //curva sul posto 1=NO
00142     vel=0.9;        //velocità base
00143     delta=0;        //
00144     dist=50;        //
00145     soglia=30;      //soglia sensore ditanza
00146     
00147     sensor.setRanges(1, 150);
00148     
00149     pc.baud(9600);                  // setto il baud rate della porta seriale pc
00150     hc05.baud(9600);                // setto il baud rate della porta rawserial hc05
00151     
00152     
00153     time_up.attach(&check_lux, 0.2);    //provvisorio
00154     //hc05.attach(&OnOff, RawSerial::RxIrq);    //definitivo
00155     //***************
00156     
00157     while(1)
00158     {
00159         //
00160     }
00161 }