funziona tutto e i motori non entrano in loop
Dependencies: mbed
Diff: MicRobot-Rev084.cpp
- Revision:
- 17:485beedd4a55
- Parent:
- 16:2581bc608372
--- 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