Revision 17:485beedd4a55, committed 2020-05-20

Comitter:

maristella

Date:

Wed May 20 18:32:35 2020 +0000

Parent:

16:2581bc608372

Commit message:

fuonziona tutto e non entra in loop

Changed in this revision

--- a/MicRobot-Rev084.cpp	Mon Mar 02 12:47:34 2020 +0000
+++ b/MicRobot-Rev084.cpp	Wed May 20 18:32:35 2020 +0000
@@ -155,6 +155,7 @@
 /* A robot fermo, il segnale di encoder non genera interrupt.                           */
 /* Questo Ticker simula l'arrivo del segnale da encoder                                 */
 /****************************************************************************************/
+/*
 void EncoderSimulate()
 {
     // ad ogni tick viene simulata la ricezione di un impulso da encoder.
@@ -173,7 +174,7 @@
     //if(!bReset)
         nCountRiseEdge++;
 } 
-
+*/
 
 /**********************************************/
 //          IRQ associata a Rx da PC 
@@ -196,7 +197,7 @@
         {
             // DIAGNOSTICA:
             // Invia Stringa di comando al Robot
-            myBLE.printf("\r\n> Prova di Trasmissione \r\n");
+            //myBLE.printf("\r\n> Prova di Trasmissione \r\n");
          }
     }
 }
@@ -392,7 +393,7 @@
             // calcola la velocità in [m/sec]. DELTAT è in [sec] lo spostamento è in [m]
             //fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT); 
             fSpeed = (fDistanzaPerStep*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT; 
-            
+                    
             // ricorda lo spostamento
             nOldCountRiseEdge = nCountRiseEdge;
            
@@ -415,7 +416,9 @@
         nCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
         fSpeed =0.0;
         fDistanzaPercorsa = 0.0;
-        myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
+        //NVIC_DisableIRQ(USART1_IRQn);
+        //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa );              
+        //NVIC_EnableIRQ(USART1_IRQn);
     }    
     //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
 }
@@ -432,8 +435,8 @@
     bCodaInMovimento = false;
   
     // messaggio di benvenuto
-    pc.printf("\r\n************  Hallo ****************** \r\n");
-    pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
+    //pc.printf("\r\n************  Hallo ****************** \r\n");
+   // pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
     
     // inizializza variabili da BLE
     cCommandBLE = 0; // inizialmente nessun comando da BLE
@@ -485,11 +488,13 @@
     
     // attiva un ticker per simulare robot in movimento. 
     //!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
-    #ifdef ENCODERSIMULATE
+ 
+    //#ifdef ENCODERSIMULATE
     // attiva il Ticker per simulare il calcolo della velocità. Ogni fDeltaTick viene simulato l'arrivo di un impulso dall'encoder del motore
-    fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
-    EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
-    #endif
+   // fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+    //EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
+    //#endif
+
     //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!!
     
     
@@ -589,6 +594,9 @@
                         Light = 0;
                         fDistanzaPercorsa = 0.0;
                         fSpeed = 0.0;
+                        NVIC_DisableIRQ(USART1_IRQn);
+                        myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa );             
+                        NVIC_EnableIRQ(USART1_IRQn);         
                     }
                     else
                     {
@@ -625,17 +633,16 @@
             fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
             fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
             // diagnostica   
-            //pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); // diagnostica   
-            //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica   
-            //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica   
-            //pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL); // diagnostica   
+            // pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); // diagnostica   
+             //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica   
+             //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica   
+            // pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL); // diagnostica   
             
             // comunica al cellulare vleocità e spostamento mentre si sta spostando
-            // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
+            //Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
             NVIC_DisableIRQ(USART1_IRQn);
-            myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
-            NVIC_EnableIRQ(USART1_IRQn);
-            
+            myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa );             
+            NVIC_EnableIRQ(USART1_IRQn);            
             // algoritmo di movimentazione delle ruote.
             if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
             {
@@ -685,7 +692,7 @@
             }
             AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
         } //if( (fX != fOldX) || (fY != fOldY))
-       
+           
         //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
     } //while (true) Ciclo principale