Diff: main.cpp

Revision:

0:9cabfa16c820

Child:

1:dea1971b7c83

Child:

2:b5a8e835fb14

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 28 10:34:24 2016 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "Motor.h"
+
+#define N true
+#define B false
+#define NUMSENS 4
+#define SPEED 0.35
+
+/*----------------------------------------------------------------------------//
+Con velocità base di 0.3
+
+//----------------------------------------------------------------------------*/
+
+
+
+//----------------------------------------------------------------------------//
+//                      Inizializzazione dei Pin                              //
+//----------------------------------------------------------------------------//
+
+Motor MotS(D9, D10, D11);   //pwm (E1), fwd (1A), rev (1B)   motore sinistro
+Motor MotD(D6, D7, D8);     //pwm (E2), fwd (2A), rev (2B)   motore destro
+DigitalIn start(PC_13);
+DigitalIn sensd[] = {D2, D3, D4, D5};        //array dei sensori digitali, da sinistra a destra
+AnalogIn sensa[] = {A0, A1, A2, A3};         //array dei sensori analogici, da sinistra a destra
+
+
+int LetturaSensore(void);
+void Avanti(void);
+void Destra(int curv);
+void Sinistra(int curv);
+void Inversione(void);
+
+Serial pc(USBTX, USBRX);
+
+//----------------------------------------------------------------------------//
+
+int stato = 0;
+
+int main()
+{
+
+    while(start==1) {}
+
+    while(1) {      
+        stato = LetturaSensore();
+        //pc.printf("stato %i\r\n", stato);
+
+        // andare avanti
+        if(stato==6) {
+            Avanti();
+        }
+        // Girare a sinistra
+        else if(stato==12||stato==14) {
+            Sinistra(0);
+        }
+        // Girare a destra
+        else if(stato==3||stato==7) {
+            Destra(0);
+        }
+        // Girare stretta a sinistra
+        else if(stato==8) {
+            Sinistra(1);
+        }
+        // Girare stretta a destra
+        else if(stato==1) {
+            Destra(1);
+        }
+        else if(stato==15) {
+            wait_ms(200);
+            if(stato==15){
+                Inversione();
+            }
+        }
+        wait_ms(150);
+    }
+}
+
+//----------------------------------------------------------------------------//
+//                              Funzioni                                      //
+//----------------------------------------------------------------------------//
+void Avanti(){
+    MotS.speed(SPEED);
+    MotD.speed(SPEED);
+}
+void Destra(int curv){
+    if(curv == 0){
+        MotS.speed(SPEED);
+        MotD.speed(SPEED-0.1);
+    }else if(curv == 1){
+        MotS.speed(SPEED+0.1);
+        MotD.speed(SPEED-0.2);   
+    }
+}
+void Sinistra(int curv){
+    if(curv == 0){
+        MotS.speed(SPEED-0.1);
+        MotD.speed(SPEED);
+    }else if(curv == 1){
+        MotS.speed(SPEED-0.2);
+        MotD.speed(SPEED+0.1);   
+    }
+}
+void Inversione(){
+    MotS.speed(-0.35);
+    MotD.speed(0.35);
+}
+
+//Funzione potenza utilizzata per la conversione da binario a decimale
+int pow(int a, int ex){
+    if(ex==0)
+        return 1;
+    if(ex==1)
+        return a;
+
+    int c=a;
+    for(; ex!=1; ex=ex-1){
+        a=a*c;
+    }
+    return a;
+}
+
+//Lettura sensore e conversione dello stato da binario->decimale
+int LetturaSensore(){
+    int data = 0;
+    for(int i=NUMSENS-1; i>=0; i=i-1)
+        data = data + sensd[i]*pow(2,NUMSENS-1-i);
+    return data;
+}
\ No newline at end of file