![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Problemi
Dependencies: mbed
Revision 19:733cea5788a4, committed 2020-07-23
- Comitter:
- francesco01
- Date:
- Thu Jul 23 10:19:54 2020 +0000
- Parent:
- 18:4b1d35d5f4fd
- Commit message:
- Problemi
Changed in this revision
MicRobot-Rev085.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/MicRobot-Rev085.cpp Wed Jul 08 19:55:47 2020 +0000 +++ b/MicRobot-Rev085.cpp Thu Jul 23 10:19:54 2020 +0000 @@ -4,43 +4,9 @@ #include "mbed.h" #include<stdlib.h> -// attivare questa #define quando si vuole simulare l'arrivo di un segnale di encoder dai motori in movimento -//#define ENCODERSIMULATE - -// pi greco -#define PI 3.14159265358979323846 - // dimensione massima del pacchetto ricevuto su seriale #define PACKETDIM 8 -// diametro della ruota in [metri] -#define DIAMETRORUOTA (0.1) - -// numero di impulsi per giro generati dall'encoder -#define IMPULSIPERGIRO 4 - -// numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m] -#define NUMCIFREDISTANZAPERCORSA 7 - -// numero di cifre con cui si vuole rappresentare la velocità in [m/s]. NUMCIFRESPEED = 5, significa che la velocità è rappresentata come xxx.xx [m/s] -#define NUMCIFRESPEED 7 - -// intervallo di tempo in [sec], in cui vengono contati gli impulsi di encoder per il calcolo della velocità -#define DELTAT (0.5) - - -// Parametri moltiplicativi. Queste operazioni vengono fatte una sola volta, evitando di farle ad ogni ciclo -#define fDistanzaPerStep (PI*DIAMETRORUOTA/IMPULSIPERGIRO) - - -// Ogni Ticker viene calcolata la velocità. Se il ticker viene richiamato ogni DELTAT sec, la velocità potrà essere calcolata come v = spazio/DELTAT -Ticker SpeedCalculateTicker; - -//!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!! -#ifdef ENCODERSIMULATE -Ticker EncoderSimulateTicker; // Ticker per simulare un segnale proveniente da encoder sul motore -#endif -//!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!! // Definizione periferica USB seriale del PC Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 @@ -56,29 +22,6 @@ DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5 -// output digitale per pilotaggio illuminazione a LED -DigitalOut Light(PA_0); -//DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 -InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore. - -// variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot -volatile int nCountRiseEdge; -volatile int nOldCountRiseEdge; - -// Input/Output -DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore -PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore -//DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore -DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore -DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore - -DigitalOut AntOutBI1 (PB_3); // Output 1 per pilotaggio input BI1 del Motore B Anteriore -PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore -//DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore -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 //carattere di comando ricevuto dal BLE e relativo parametro volatile char cCommandBLE; // cambia nella routine di interrupt @@ -89,33 +32,6 @@ char cOldCommandBLE; int nOldParamBLE; -// coordinate polari del joystick sulla APP, fornite dalla routine di interrupt -volatile double fTeta; -volatile double fRo; -volatile int nRo; -volatile int nTeta; - -// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt -volatile double fX, fY; -// memorizza ultimi valori delle coordinate del Joystick -double fOldX, fOldY; - -// variabili ausiliarie per l'algoritmo di posizionamento -double fV, fW; - -// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore -double fR, fL; - -// distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore -volatile double fDistanzaPercorsa; // calcolata nel main, utilizzata nelle IRQ - -// velocità calcolata gli impulsi contati in un intervallo DELTAT msec -volatile double fSpeed; // calcolata nel main, utilizzata nelle IRQ - -// Scopi diagnostici: Ogni fDeltaTick viene simulata la generazione di un impulso di encoder. -// velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s] -double fDeltaTick; - // indice per i cicli int nIndex; @@ -130,49 +46,9 @@ // flag che indica se il sw è in Reset volatile bool bReset; -// flag che indica se la coda è in movimento/ferma true/false -volatile bool bCodaInMovimento; - /**************************************************************************************/ /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */ /**************************************************************************************/ -void riseEncoderIRQ() -{ - // incrementa il contatore di impulsi contati, se il sw non è resettato, cioè se bReset = false - //if(!bReset) - nCountRiseEdge++; - - //pc.printf("Sono qui 0 \n\r"); // diagnostica -} - - -/****************************************************************************************/ -/* Diagnostica: */ -/* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO */ -/* Routine di gestione del ticker per simulare encoder */ -/* Simula il segnale di encoder ricevuto con un determinato DELTAT */ -/* 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. - // Esempio: - // fDeltaTick = 0.05 sec - // diametro ruota, DIAMETRORUOTA = 0.1 metri - // circonferenza ruota = 0.1*3.14= 0.314 metri - // impulsi per giro dall'encoder, IMPULSIPERGIRO = 4 - // un tick simula l'arrivo di un impulso da encoder e quindi simula la percorrenza di 1/4 di circonferenza - // ogni volta che arriva un tick simulato da encoder, si presume di aver percorso circonferenza/4 = 0.314/4 = 0.0785 metri - // il tick arriva ogni fDeltaTick secondi e a ogni tick percorro 0.0785 metri -> velocità = 0.0785/0.05 = 1.57 [m/s] - // spostamento = (Spazio per ogni tick)/(tempo per ogni tick) - // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s] - - // simula impulso inviato dall'encoder se il sw non è resettato, cioè se bReset = false - //if(!bReset) - nCountRiseEdge++; -} - /**********************************************/ // IRQ associata a Rx da PC @@ -191,12 +67,12 @@ //pc.putc(cReadChar); // Diagnostica: write char to PC //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica - if(cReadChar == '0') // se scrivo '0', invia questa stringa + /* if(cReadChar == '0') // se scrivo '0', invia questa stringa { // DIAGNOSTICA: // Invia Stringa di comando al Robot myBLE.printf("\r\n> Prova di Trasmissione \r\n"); - } + }*/ } } @@ -215,14 +91,14 @@ cReadChar = myBLE.getc(); // Read character //caRxPacket[nCharCount]=cReadChar; //nCharCount++; - pc.printf("%c", cReadChar); // diagnostica + // pc.printf("%c", cReadChar); // diagnostica // acquisisce il carattere di start comando o coordinate da APP if(cReadChar=='(') { // acquisisce il primo carattere di comando o di coordinate cReadChar = myBLE.getc(); // Read character - pc.printf("%c", cReadChar); // diagnostica + // pc.printf("%c", cReadChar); // diagnostica // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++ // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T @@ -232,160 +108,44 @@ cCommandBLE = cReadChar; // legge e memorizza come paramentro il successivo carattere cReadChar = myBLE.getc(); // legge parametro - pc.printf("%c", cReadChar); // diagnostica + // pc.printf("%c", cReadChar); // diagnostica nParamBLE = cReadChar-0x30; cReadChar = myBLE.getc(); // legge la ')' di chiusura comando - pc.printf("%c", cReadChar); // diagnostica + //pc.printf("%c", cReadChar); // diagnostica // visualizza comando e parametro inviato da BLE - pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica + // pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica + if(cReadChar==')') + { + //pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica + } + else // dopo la Y e i numeri, mi attendo parentesi chiusa ) + { + pc.printf("> Errore = %c invece di ) \r\n\n", cReadChar); // diagnostica + } } // +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++ - else - { - if(cReadChar=='X') // è stato acquisisto carattere X di inizio valori numeri dell'ascissa? - { - //+++++++++ INIZIO acquisisce il valore dell'ascissa Xnnn +++++++++++++++ - nCharCount = 0; - do - { - cReadChar = myBLE.getc(); // Read character - caRxPacket[nCharCount]=cReadChar; - nCharCount++; - pc.printf("%c", cReadChar); // diagnostica - } - while((cReadChar >= 0x30) && (cReadChar <= 0x39)); - //+++++++++ FINE acquisisce il valore dell'ascissa Xnnn +++++++++++++++ - - //+++++++++ INIZIO converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++ - nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y' - fX=0; - for(nIndex =0; nIndex <= nCharCount; nIndex++) - { - fX = fX + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex)); - } - //+++++++++ FINE converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++ - - // verifica se l'ultimo carattere ricevuto dopo Xnnn è stato Y - if(cReadChar=='Y') - { - //+++++++++ INIZIO acquisisce il valore dell'ordinata Ynnn +++++++++++++++ - nCharCount = 0; - do - { - cReadChar = myBLE.getc(); // Read character - caRxPacket[nCharCount]=cReadChar; - nCharCount++; - pc.printf("%c", cReadChar); // diagnostica - } - while((cReadChar >= 0x30) && (cReadChar <= 0x39)); - //+++++++++ FINE acquisisce il valore dell'ordinata Ynnn +++++++++++++++ - - //+++++++++ INIZIO converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++ - nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y' - fY=0; - for(nIndex =0; nIndex <= nCharCount; nIndex++) - { - fY = fY + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex)); - } - //+++++++++ FINE converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++ - - // se riceve la coda del comando ), stampa le coordinate ricevute - if(cReadChar==')') - { - pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica - - // trasporta x e y nei range desiderati - fY= 100 - fY; - fX = fX - 100; - - pc.printf("\n\r(fX , fY) traslate = (%.1f , %.1f) \r\n\n", fX, fY); // diagnostica - } - else // dopo la Y e i numeri, mi attendo parentesi chiusa ) - { - pc.printf("> Errore = %c invece di ) \r\n\n", cReadChar); // diagnostica - } - } - else // dopo la X e i numeri mi aspetto Y - { - pc.printf("> Errore = %c invece di Y \r\n\n", cReadChar); // diagnostica - } - } - else // dopo la ( mi aspetto Y - { - pc.printf("> Errore = %c invece di X \r\n\n", cReadChar); // diagnostica - } - } // if(comandi alfanumerici) } // if(cReadChar == '(') - } + }// chiude il while } + + + /*********************************************************************************************************************************************/ /* ogni DELTAT secondi scatta questo ticker. */ /* Tra due Tick viene contato il numero di mpulsi impulsi di encoder ricevuti con degli interrupt e contentuo nella variabile nCountRiseEdge */ /*********************************************************************************************************************************************/ -void SpeedCalculate() -{ - - // se bReset = true non fare nessun calcolo della velocità e spostamento e azzera velocità e spostamento - if(!bReset) - { - //pc.printf("Sono qui 1 \n\r"); // diagnostica - //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++ - //nCountRiseEdge++; //----diagnostica - // se nella IRQ, durante il periodo di calcolo della velocità, sono stati contati fronti di salita dell'encoder, il robot si sta muovendo - if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo - { - //pc.printf("nCountRiseEdge= %d ; nOldCountRiseEdge= %d \n\r", nCountRiseEdge, nOldCountRiseEdge); // diagnostica - - // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati) - fDistanzaPercorsa = fDistanzaPerStep*nCountRiseEdge; - - // 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; - - // comunica al cellulare vleocità e spostamento mentre si sta spostando - //PRIMA ERA QUI ma si bloccava myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); - } - else - { - // se non ci sono variazioni di impulsi, il robot è fermo, la velocità è 0.0 - fSpeed= 0.0; - } - - //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); // diagnostica - } - else - { - // bReset = true - // comunica al cellulare vleocità e spostamento nulli - nOldCountRiseEdge=0; // non ci sono variazioni di numero di impulsi - 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 ); - } - //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++ -} - - /**********/ /* MAIN */ /**********/ int main() -{ - // inizializza PWM del motore coda - MotoreCoda.period_ms(50); // periodo PWM - bCodaInMovimento = false; - +{ // messaggio di benvenuto - pc.printf("\r\n************ Hallo ****************** \r\n"); - pc.printf("*** Modulo di Ispezione Condutture ***\r\n"); + pc.printf("\r\n************ CLM ****************** \r\n"); + pc.printf("*** Campanella Amaldi ***\r\n"); + myBLE.printf("***W Campanella Amaldi ***\r\n"); // inizializza variabili da BLE cCommandBLE = 0; // inizialmente nessun comando da BLE @@ -393,94 +153,12 @@ cParamBLE = 0; // inizialmente nessun parametro da BLE nParamBLE=0; // inizialmente nessun parametro da BLE nOldParamBLE=0; // inizialmente nessun parametro da BLE - fX = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0) - fOldX = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0) - fY = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0) - fOldY = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0) bReset = false; //bReset = true/false quando riceve un comando (R1)/(R0) dalla APP - - // inizializza variabili - fDistanzaPercorsa = 0.0; - fSpeed = 0.0; // inizializza array di caratteri ricevuti for(nIndex=0; nIndex < PACKETDIM; nIndex++) {caRxPacket[nIndex]=0;} - nCharCount=0; - - - // inizializza i valori di modulo e fase ricevuti dal joystick - nRo = 0; - nTeta = 0; - - //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi - // gli impulsi di encoder vengono contati da una IRQ collegata all'input da encoder - // ogni DELTAT secondi scatta un ticker che calcola la velocità - - // definisci il mode del segnale digitale di EncoderA - InEncoderA.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(); - SpeedCalculateTicker.attach(&SpeedCalculate,DELTAT); - //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - // Attiva la IRQ per la RX su seriale e sulla Rx della BLE - myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla seriale del BLE - pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC - - // 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 - // 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 - //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!! - - - //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++ - /* - //pc.printf("Coda in movimento \n\r"); //diagnostica - while(true) - { - - if ( ( fX != 0 ) || (fY != 0)) - { - if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma - { - MotoreCoda.write (0.6); - bCodaInMovimento = true; - pc.printf("Coda in movimento \n\r"); - } - } - else - { - // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0 - if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento - { - pc.printf("Coda ferma \n\r"); - MotoreCoda.write (0.0); - bCodaInMovimento = false; - // comunica al cellulare vleocità nulla - // Disattiva/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", 100.0, 1000 ); - //NVIC_EnableIRQ(USART1_IRQn); - - } - } - - } - */ - //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++ - + nCharCount=0; //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++ @@ -490,62 +168,39 @@ { //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++ - - if ((fX!=0) || (fY!=0)) //la coda non si muove se il Joystick è nella posizione (0,0) - { - // il joystick è in posizione diversa da (0,0), fai muovere la coda - if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma - { - //pc.printf("Coda in movimento \n\r"); //diagnostica - MotoreCoda.write (0.4); - bCodaInMovimento = true; - - } - } - else - { - // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0 - if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento - { - //pc.printf("Coda ferma \n\r"); //diagnostica - MotoreCoda.write (0.0); - bCodaInMovimento = false; - // comunica al cellulare vleocità nulla - // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE - //NVIC_DisableIRQ(USART1_IRQn); - //myBLE.printf("Speed= 0.0 [m/s]; Trip= %.2f [m]\n\r", fDistanzaPercorsa ); - //NVIC_EnableIRQ(USART1_IRQn); - - } - } - + if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE)) { switch (cCommandBLE) { - case 'T': // accendi/spegni LED su scheda - { - myLed = nParamBLE; + case 'A': // entrata + { + if(nParamBLE==1) + { + myBLE.printf("Entrate\n\r"); + } + else + { + } } break; - case 'L': // Accendi/spegni illuminazione a LED + case 'B': // Cambio { - Light = nParamBLE; + if(nParamBLE==1) + { + pc.printf("Cambio\n\r"); + } + else + { + } } break; - case 'R': // Reset odometria e illuminazione + case 'C': // Uscita { if(nParamBLE==1) { - bReset = true; - nCountRiseEdge = 0; - nOldCountRiseEdge = 0; - Light = 0; - fDistanzaPercorsa = 0.0; - fSpeed = 0.0; + pc.printf("Uscite\n\r"); } else { - // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente - bReset = false; } } break; @@ -557,88 +212,6 @@ } //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++ - //Invert X - //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y - //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X - //Calcola R: R = (V+W) /2 - //Calcola L: L= (V-W)/2 - //Scala i valori di L e R in base all'hardware. - //invia i valori al robot. - // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote - if( (fX != fOldX) || (fY != fOldY)) - { - fOldX = fX; - fOldY = fY; - // algoritmo di conversione dalla posizione del Joystick (fX, fY) alla velocità delle ruote (fR, fL) - fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio - fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio - 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 - - // comunica al cellulare vleocità e spostamento mentre si sta spostando - // 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); - - // algoritmo di movimentazione delle ruote. - if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore - { - fR =-fR; - // Vai indietro - PostOutBI1 = 1; - PostOutBI2 = 0; - } - else - { - if(fR >0) - { - // Vai avanti - PostOutBI1 = 0; - PostOutBI2 = 1; - } - else - { - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - } - } - PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore) - if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore - { - fL =-fL; - // Vai indietro - AntOutBI1 = 1; - AntOutBI2 = 0; - } - else - { - if(fL >0) - { - // Vai avanti - AntOutBI1 = 0; - AntOutBI2 = 1; - - } - else - { - // spegni - AntOutBI1 = 0; - AntOutBI2 = 0; - } - } - 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 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++