Diff: main.cpp

Revision:

2:b5a8e835fb14

Parent:

0:9cabfa16c820

--- a/main.cpp	Mon Nov 28 10:34:24 2016 +0000
+++ b/main.cpp	Mon Dec 05 10:39:52 2016 +0000
@@ -4,31 +4,25 @@
 #define N true
 #define B false
 #define NUMSENS 4
-#define SPEED 0.35
-
-/*----------------------------------------------------------------------------//
-Con velocità base di 0.3
-
-//----------------------------------------------------------------------------*/
-
-
+#define SPEED 0.5
 
 //----------------------------------------------------------------------------//
 //                      Inizializzazione dei Pin                              //
 //----------------------------------------------------------------------------//
 
-Motor MotS(D9, D10, D11);   //pwm (E1), fwd (1A), rev (1B)   motore sinistro
+Motor MotS(D9, D10, D12);   //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
-
+PwmOut led[] = {D11, D14, D15};        //array dei sensori digitali, da sinistra a destra
 
 int LetturaSensore(void);
 void Avanti(void);
 void Destra(int curv);
 void Sinistra(int curv);
 void Inversione(void);
+void ledrgb(int st);
 
 Serial pc(USBTX, USBRX);
 
@@ -37,10 +31,31 @@
 int stato = 0;
 
 int main()
-{
+{   
+    // Inizializzazione PWM
+    for(int i=0; i<=2; i++)
+        led[i].period(0.01);
+    
+    wait(1);
+    
+    // Inizializzazione macchinina
+    for(int i=0; i<3; i++){
+        for(int j=0; j<3; j++){
+            led[0] = 0;
+            led[1] = 0;
+            led[2] = 0;
 
-    while(start==1) {}
-
+            led[j] = 1;
+            wait_ms(150);
+        }
+    }
+    
+    // Pulsante di Start
+    while(start==1){
+        ledrgb(stato);    
+    }
+    
+    // Ciclo principale
     while(1) {      
         stato = LetturaSensore();
         //pc.printf("stato %i\r\n", stato);
@@ -65,13 +80,14 @@
         else if(stato==1) {
             Destra(1);
         }
+        // Inversione
         else if(stato==15) {
-            wait_ms(200);
+            wait_ms(100);
             if(stato==15){
                 Inversione();
             }
         }
-        wait_ms(150);
+        ledrgb(stato);
     }
 }
 
@@ -88,7 +104,7 @@
         MotD.speed(SPEED-0.1);
     }else if(curv == 1){
         MotS.speed(SPEED+0.1);
-        MotD.speed(SPEED-0.2);   
+        MotD.speed(SPEED-0.3);   
     }
 }
 void Sinistra(int curv){
@@ -96,7 +112,7 @@
         MotS.speed(SPEED-0.1);
         MotD.speed(SPEED);
     }else if(curv == 1){
-        MotS.speed(SPEED-0.2);
+        MotS.speed(SPEED-0.3);
         MotD.speed(SPEED+0.1);   
     }
 }
@@ -125,4 +141,16 @@
     for(int i=NUMSENS-1; i>=0; i=i-1)
         data = data + sensd[i]*pow(2,NUMSENS-1-i);
     return data;
+}
+
+void ledrgb(int st){
+        if (st == 0){
+            led[0] = 1;   
+            led[1] = 0;    
+            led[2] = 1;
+        }else{
+            led[0] = sensa[0];   
+            led[1] = (sensa[1]+sensa[2])/2;    
+            led[2] = sensa[3];
+        }
 }
\ No newline at end of file