Diff: MicRobot-Rev07_FUNZIONA.cpp

Revision:

12:f1dc27a8c012

Parent:

11:da53d3e94a41

--- a/MicRobot-Rev07_FUNZIONA.cpp	Sat Feb 08 09:17:12 2020 +0000
+++ b/MicRobot-Rev07_FUNZIONA.cpp	Tue Feb 11 22:20:06 2020 +0000
@@ -79,7 +79,7 @@
 DigitalOut AntOutBI2 (PB_4);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
 DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
 
-PwmOut MotoreCoda (PB_8);  // Output movimento coda
+PwmOut MotoreCoda (PB_9);  // Output movimento coda
 
 //carattere di comando ricevuto dal BLE e relativo parametro
 volatile char cCommandBLE; // cambia nella routine di interrupt
@@ -170,6 +170,9 @@
 // contatore di caratteri ricevuti daBLE
 volatile int nCharCount; 
 
+//DigitalIn prova(PC_1, PullUp);
+
+
 /**************************************************************************************/
 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
 /**************************************************************************************/
@@ -374,8 +377,86 @@
 /**********/
 int main()
 {  
-   MotoreCoda.period_ms(50);  // periodo PWM
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
    
+    //++++++++++++++++++++++++++ INIZIO Test Interrupt da encoder ++++++++++++++++++++++++++
+    /*
+    // definisci il mode del segnale digitale di EncoderA
+    InEncoderA.mode(PullUp);
+    //prova.mode(PullUp);
+    // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
+    InEncoderA.rise(&riseEncoderIRQ);
+    // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
+    nCountRiseEdge=0;
+    nOldCountRiseEdge=0;               
+    
+    InEncoderA.enable_irq();
+    
+    while(true)
+    { 
+      printf("nCountRiseEdge=%d\r\n", nCountRiseEdge);
+        
+    }
+    */
+    //++++++++++++++++++++++++ FINE  Test Interrupt da encoder +++++++++++++++++++++++
+    
+    //++++++++++++ INIZIO Raw Test Motore ++++++++++++
+    /*
+    while(1)
+    {
+        //if(myButton == 0)
+        {
+            // CW
+            pc.printf("CW\r\n\r\n");
+            PostOutPWB = 1;
+            PostOutBI1 = 1;
+            PostOutBI2 = 0;    
+            AntOutPWB = 1;
+            AntOutBI1 = 0;
+            AntOutBI2 = 1;    
+            wait (1);
+            
+            
+            // spegni 
+            PostOutPWB = 1;
+            PostOutBI1 = 0;
+            PostOutBI2 = 0; 
+            AntOutPWB = 1;
+            AntOutBI1 = 0;
+            AntOutBI2 = 0; 
+            wait (2);
+            
+
+            // CCW
+            pc.printf("CCW\r\n\r\n");
+            PostOutPWB = 1;
+            PostOutBI1 = 0;
+            PostOutBI2 = 1;
+            AntOutPWB = 1;
+            AntOutBI1 = 1;
+            AntOutBI2 = 0;
+            wait (2);
+            
+            
+            // spegni 
+            PostOutPWB = 1;
+            PostOutBI1 = 0;
+            PostOutBI2 = 0;
+            AntOutPWB = 1;
+            AntOutBI1 = 0;
+            AntOutBI2 = 0; 
+            wait (1);
+            
+        }
+    } // while(true)  Raw test motore
+    */
+    //++++++++++++ FINE Raw Test Motore ++++++++++++
+
+    MotoreCoda.period_ms(50);  // periodo PWM
+    
+    
     // messaggio di benvenuto
     pc.printf("\r\n************  Hallo ****************** \r\n");
     pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
@@ -455,10 +536,10 @@
     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    
-    
     while(true)
     {
+        //pc.printf("nCountRiseEdge=%d\r\n", nCountRiseEdge);
+        
         //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
         if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
         {
@@ -565,15 +646,17 @@
                 }
             }
             AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
+            printf("nCountRiseEdge=%d\r\n", nCountRiseEdge);
+            if ( (fY==0) && (fX==0))    //La coda non si muove se il joystick è nella posizione (0,0)
+            {
+                MotoreCoda.write (0.0);    // velocità   ***    max 1.0
+            } 
+            else  //La coda si muove se il joystick non è nella posizione (0,0)
+            {
+                MotoreCoda.write (0.4);    // velocità   ***    max 1.0
+            } 
         }
-        if ( nCountRiseEdge != 0)    //La coda si muove quando registra impulso 
-                 {
-                    MotoreCoda.write (0.4);    // velocità   ***    max 1.0
-                 } 
-                else               //La coda non si muove quando non registra impulso 
-                 {
-                    MotoreCoda.write (0.0);    // velocità   ***    max 1.0
-                 } 
+       
         //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
     } //while (true) Ciclo principale