Diff: main.cpp

Revision:

0:a962eb4d6d61

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 04 15:15:57 2016 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "HCSR04.h"
+
+Motor mSx(D10, D8, D9);     //pwm (E1), fwd (1A), rev (1B)
+Motor mDx(D5, D6, D7);      //pwm (E2), fwd (2A), rev (2B)
+
+HCSR04 sensor(D11, D12);    //echo pin, trig pin
+
+AnalogIn luxSx(A2);         //PhotoR Sx
+AnalogIn luxFw(A1);         //PhotoR Fw
+AnalogIn luxDx(A0);         //PhotoR Dx
+
+//DigitalOut ledSx(D2);
+//DigitalOut ledFw(D3);
+//DigitalOut ledDx(D4);
+
+Serial pc (SERIAL_TX, SERIAL_RX);
+
+RawSerial hc05(D13, D14);
+
+Ticker time_up; 
+
+//DigitalIn button(PC_13);
+
+float inv;
+float vel;
+float delta;
+float dist;
+float soglia;
+char  a;
+
+void forward(float v) //v rta 0 e 1
+{
+    mDx.speed(v);//DX
+    mSx.speed(v); //SX
+    
+    //ledSx=0;
+    //ledFw=1;
+    //ledDx=0;
+    
+    pc.printf("Fw");
+}
+
+void turnDx (float v, float t) //t tra 0 e 1
+{
+    mDx.speed(v*t);//DX
+    mSx.speed(v); //SX
+    
+    //ledSx=0;
+    //ledFw=0;
+    //ledDx=1;
+    
+    pc.printf("Dx");
+}
+void turnSx (float v, float t)
+{    
+    mDx.speed(v);//DX
+    mSx.speed(v*t); //SX
+    
+    //ledSx=1;
+    //ledFw=0;
+    //ledDx=0;
+    
+    pc.printf("Sx");
+}
+
+void check_lux()
+{
+    float Sx=luxSx.read();
+    float Fw=luxFw.read();
+    float Dx=luxDx.read();
+    
+    dist=50;           //provvisorio
+    //dist=sensor.getDistance_cm();     //definitivo
+    //**************
+    
+    if(dist>soglia)
+    {
+        inv=1;
+    }
+    else
+    {
+        inv=-1;
+    }
+    
+    delta=Sx-Dx;
+    
+    if(Fw>Sx&Fw>Dx&dist>soglia)
+    {
+        forward(vel);
+    }
+    else if (delta>0)
+    {
+        turnSx(vel,inv*(1-delta));
+    }
+    else
+    {
+        turnDx(vel,inv*(delta-1));
+    }
+}
+
+void OnOff()
+{
+    if(hc05.readable())         // se e’¨ stato ricevuto un carattere 
+    {                                       
+        a=hc05.getc();          // assegna il carattere ricevuto alla var a
+        switch(a)
+        {
+            case ('s'):         //Start
+                time_up.attach(&check_lux, 0.2);
+                pc.printf("Start %c\n\r",a);
+                break;
+            case ('d'):         //Stop
+                time_up.detach();
+                pc.printf("Stop %c\n\r",a);
+                break;
+            case ('a'):         //accelera
+                if(vel<=1)
+                {
+                    vel+=0.1;
+                }              
+                break;
+            case ('f'):         //frena
+                if(vel>0)
+                {
+                    vel-=0.1;
+                }      
+                break;
+            default:
+                break;
+        }
+    }
+}
+
+
+
+
+int main()
+{
+    inv=1;          //curva sul posto 1=NO
+    vel=0.9;        //velocità base
+    delta=0;        //
+    dist=50;        //
+    soglia=30;      //soglia sensore ditanza
+    
+    sensor.setRanges(1, 150);
+    
+    pc.baud(9600);                  // setto il baud rate della porta seriale pc
+    hc05.baud(9600);                // setto il baud rate della porta rawserial hc05
+    
+    
+    time_up.attach(&check_lux, 0.2);    //provvisorio
+    //hc05.attach(&OnOff, RawSerial::RxIrq);    //definitivo
+    //***************
+    
+    while(1)
+    {
+        //
+    }
+} 
\ No newline at end of file