Test Comm
Dependencies: mbed
Revision 8:7a81e89f60df, committed 2022-05-18
- Comitter:
- pinofal
- Date:
- Wed May 18 14:29:48 2022 +0000
- Parent:
- 7:7043da244e4b
- Commit message:
- Test comm
Changed in this revision
Mic-Rev03.cpp | Show diff for this revision Revisions of this file |
SITAEL-Comm.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Mic-Rev03.cpp Thu Aug 08 09:04:07 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,839 +0,0 @@ -// pilotaggio carrello tramite BLE. -// testato su L476RG e F401RE - -#include "mbed.h" -#include<stdlib.h> - - -// pi greco -#define PI 3.14159265358979323846 - -// 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 5 - -// 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 5 - -// intervallo di tempo in [msec], cui vengono contati gli impulsi di encoder per il calcolo della velocità -#define DELTAT 200 - -// ticker per il calcolo della velocità -//Ticker SpeedTicker; - -// Timer per il calcolo della velocità. Nel periodo di tempo del timer, conta gli impulsi ricevuti come interrupt dall'encoder -Timer TimerSpeed; - -// tempo inizio intermedio e fine del timer che misura il tempo per il calcolo della velocità -int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow; - -// Ticker per la simulazione di segnale proveniente da encoder sul motore -Ticker SpeedTicker; - -// Definizione periferica USB seriale del PC -Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 - -// Definizione periferica seriale del Modulo BLE ELETT114A -Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps - -// Input di Reset per il Modulo BLE HC-05 -DigitalOut BleRst(PA_8); - -// User Button, LED -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. - -// carattere letto dalla seriale del PC -char cReadChar; - -// DutyCycle del segnale PWM -float fDutyCycle; - -// 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_4); // 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_3); // 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 - - -//carattere di comando ricevuto dal BLE -volatile char cCommandBLE; // cambia nella routine di interrupt -// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente -char cOldCommandBLE; - -// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt -volatile double fTeta; -volatile double fRo; - -// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt -volatile double fX, fY; -// memorizza ultimi valori delle coordinate del joistick -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 -double fDistanzaPercorsa; - -// velocità calcolata gli impulsi contati in un intervallo DELTAT msec -double fSpeed; - -// Scopi diagnostici: Ogni fDeltaTick viene simulata la generazione di un impulso di encoder. -// velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s] -double fDeltaTick; - -// distanza percorsa e relativo indice, calcolata in [m] e trasformata in caratteri | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m] -//char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA]; -//int nIndexDistanzaPercorsa; - -//**********************************************/ -// IRQ associata a Rx da BLE -//**********************************************/ -void BLERxInterrupt(void) -{ - // array per la ricezione dei messaggi da seriale - char cReadChar; - - //indice per i cicli - int nIndex; - - // array per la ricezione dei messaggi da seriale - char caRxPacket[8]; - //int nRxPacketSize; - - // variabile per estrarre le cifre della distanza percorsa. La distanza Percorsa viene calcolata nel MAIN con la varibile fDistanzaPercorsa - int nDistanzaPercorsa; - // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m] - char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA]; - int nIndexDistanzaPercorsa; - - // variabile per estrarre le cifre della velocità di percorrenza. La velocità viene calcolata nel MAIN con la varibile fSpeed - int nSpeed; - // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m] - char caSpeed[NUMCIFRESPEED]; - int nIndexSpeed; - - - //pc.printf("BLE RxInterrupt: \n\r"); - - // ricevi caratteri su seriale, se disponibili - while((myBLE.readable())) - { - // acquisice stringa in input e relativa dimensione - cReadChar = myBLE.getc(); // Read character - if(cReadChar == 0x02) - { - - //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6 - - //-- command will be 8 bytes for joystick values - //-- command will be 3 bytes for button change event - //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03) - - - caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX - cReadChar = myBLE.getc(); // legge il secondo carattere - if(cReadChar > 0x40) - { - // Button: - //-- Button events send a single character in a 3-byte packet - //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX> - //-- B2 uses "C" for changed to on, "D" for changed to off - //-- B3..B6 follow in order; valid button even characters are "A".."L" - caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP - caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX - // passa il comando ricevuto nella variabile globale - cCommandBLE = caRxPacket[1]; - - // Diagnostica - /* - pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6 - */ - //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - } - else - { - //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggi Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // Joystick: - //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis - //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator - //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX> - caRxPacket[1] = cReadChar; // memorizza il secondo carattere - for(nIndex=2; nIndex<8; nIndex++) - { - caRxPacket[nIndex] = myBLE.getc(); - } - // dal messaggio estrae e visualizza le coordinate cartesiane - fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200); - fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200); - // converte la posizione del joistick in coordinate polari - fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo - if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360° - fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100 - - // diagnostica - /* - pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE - pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane - pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari - */ - } - //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //fDistanzaPercorsa = 980.20; // diagnostica - nDistanzaPercorsa = fDistanzaPercorsa*100; // moltiplica per 100 per considerare fino ai centesimi della distanza percorsa - nSpeed = fSpeed*100; // moltiplica per 100 per considerare fino ai centesimi della velocità - - // inizializza caDistanzaPercorsa[] - for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++) - { - caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0'; - } - - // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale - nIndexDistanzaPercorsa = 0; - do - { - // genera digits della distanza percorsa - caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0'; - - //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica - nIndexDistanzaPercorsa++; - } - while ((nDistanzaPercorsa /= 10) > 0) ; - // Diagnostica: visualizza la distanza percorsa sul PC - //pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica - //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // inizializza caSpeed[] - for(nIndexSpeed = 0; nIndexSpeed < NUMCIFREDISTANZAPERCORSA; nIndexSpeed++) - { - caSpeed[nIndexSpeed] = '0'; - } - - // estrae le singole cifre di fSpeed, fino alla seconda cifra decimale - nIndexSpeed = 0; - do - { - // genera digits della velocità - caSpeed[nIndexSpeed] = nSpeed % 10 + '0'; - - //pc.printf("caSpeed[%d]: %c \n\r", nIndexSpeed, caSpeed[nIndexSpeed]); // diagnostica - nIndexSpeed++; - } - while ((nSpeed /= 10) > 0) ; - //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - //++++++++++++++++++++++++++++++++++++++++++++ INIZIO Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - // Diagnostica: visualizza la velocità sul PC - //pc.printf("Speed: %c,%c,%c,%c,%c \n\r", caSpeed[4], caSpeed[3], caSpeed[2], caSpeed[1], caSpeed[0]); // diagnostica - - // Struttura Nominale del comando da inviare al robot STX ,B6, B5, B4, B3 B2 B1 0x01,Data1,...........................................................................................................,0x04,Data2,.....................................................,0x05,Data3,...................,ETX; - myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,' ',' ',' ',' ',' ',' ',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,caSpeed[4],caSpeed[3],caSpeed[2], ',',caSpeed[1],caSpeed[0],0x05,0x30,0x30,0x30,0x30,0x30, 0x03); - - //++++++++++++++++++++++++++++++++++++++++++++ FINE Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - } - } -} - -/**********************************************/ -// IRQ associata a Rx da PC -//**********************************************/ -void pcRxInterrupt(void) -{ - // array per la ricezione dei messaggi da seriale - char cReadChar; - - // ricevi caratteri su seriale, se disponibili - while((pc.readable())) - { - // acquisice stringa in input e relativa dimensione - cReadChar = pc.getc(); // read character from PC - //myBLE.putc(cReadChar); // Diagnostica: write char to BLE - //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 - { - // DIAGNOSTICA: - // pc.printf("W>: Inviato comando a BLE\n\r"); // diagnostica - //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX> - //-- button status is binary formatted string (no indicator) - //-- data fields sent as strings - //-- send empty string to unused field (not sure if short response packet is allowed without additional testing) - // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX; - myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03); - } - } -} - - -/**************************************************************************************/ -/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */ -/**************************************************************************************/ -void riseEncoderIRQ() -{ - nCountRiseEdge++; -} - -/****************************************************************************************/ -/* 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 SpeedCalculate() -{ - // ad ogni tick viene ricevuto un impulso da encoder. - // velocità = (Spazio per ogni tick)/(tempo per ogni tick) - // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s] - - // simula impulso inviato dall'encoder - nCountRiseEdge++; -} - - -/**********/ -/* MAIN */ -/**********/ -int main() -{ - - // messaggio di benvenuto - 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 - cOldCommandBLE = 0; // inizialmente nessun comando da BLE - fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) - fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) - fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) - fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) - - // inizializza il BLE - BleRst = 0; - wait_ms(100); - BleRst = 1; - cCommandBLE = '0'; - cOldCommandBLE = '0'; - - // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore - PostOutPWB.period_us(10); // periodo del PWM Posteriore - PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore) - AntOutPWB.period_us(10); // periodo del PWM Anteriore - AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore) - - // inizializza variabili - fDistanzaPercorsa = 0.0; - fSpeed = 0.0; - - // Attiva la IRQ per la RX su seriale - 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 +++++++++++++ - /* - fDeltaTick = 0.05; // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s] - SpeedTicker.attach(&SpeedCalculate,fDeltaTick); // Diagnostica - */ - //+++++++++ FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++ - - //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // 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; - //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - - //++++++++++++++++++++++++++++++++++++++++++++++++++++ - //++++++++++++++ INIZIO Ciclo Principale +++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++ - - while(true) - { - //+++++++++++++++++++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++++++++++++++++++++++ - - // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi la velocità di spostamento del robot - InEncoderA.enable_irq(); - - // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi - TimerSpeed.start(); - nTimerStart=TimerSpeed.read_ms(); - - // per 100ms conta gliimpulsi sull'encoder - while( (nTimerCurrent-nTimerStart) < DELTAT) // attende il passare di DELTAT millisec - { - nTimerCurrent=TimerSpeed.read_ms(); - // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart)); - } - TimerSpeed.stop(); // ferma il timer - - // disabilita interrupt sul segnale di encoder per contare il numero di impulsi. Disattivandolo, si evita di continuare a mandare in interrupt il processore - InEncoderA.enable_irq(); - //+++++++++++++++++++++++++++++ FINE calcola spostamento con encoder sul motore ++++++++++++++++++++++++++++++++++++++++ - - //+++++++++++++++++++++++++ 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 - { - // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati) - fDistanzaPercorsa = (PI*DIAMETRORUOTA/IMPULSIPERGIRO)*nCountRiseEdge; - - // calcola la velocità in [m/sec]. DELTAT è in [msec] lo spostamento è in [m] - fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/float(DELTAT/1000.0); - - // ricorda lo spostamento - nOldCountRiseEdge = nCountRiseEdge; - } - //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++ - - //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++ - if(cCommandBLE != cOldCommandBLE) - { - switch (cCommandBLE) - { - case 'A': // accendi LED su scheda - { - myLed = 1; - }; break; - case 'B': // spegni LED su scheda - { - myLed = 0; - }; break; - case 'C': // accendi illuminazione a LED - { - Light = 1; - }; break; - case 'D': // spegni illuminazione a LED - { - Light = 0; - }; break; - case 'E': // Reset odometria e illuminazione - { - nCountRiseEdge = 0; - nOldCountRiseEdge = 0; - Light = 0; - fDistanzaPercorsa = 0.0; - fSpeed = 0.0; - myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,'0','0','0',',','0','0',0x04,'0','0','0', ',','0','0',0x05,0x30,0x30,0x30,0x30,0x30, 0x03); - }; break; - case 'F': // Toglie Reset da odometria e illuminazione - { - }; break; - - default: break; - } - pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica - cOldCommandBLE = cCommandBLE; - } - //++++++++++++++++++++++++++++++++++++++++++++ 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 joistick (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); - pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); - pc.printf("> Velocita' Right R = %.2f\r\n", fR); - pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); - - // 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) - } - //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++ - } //while (true) Ciclo principale - - //++++++++++++++++++++++++++++++++++++++++++++++++++ - //++++++++++++++ FINE Ciclo Principale +++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++ - - - - - //++++++++++++++++++++++++++++++++++++++++++++++++++++ - //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++ - - //++++++++++++++ INIZIO Test di calcolo e trasmissione distanza percorsa +++++++++++++++++++++++++++++++ - /* - pc.printf("myButton = 1; iniziale\r\n"); - while(myButton == 1); - pc.printf("myButton = 0; \r\n"); - while(myButton == 0); - pc.printf("myButton = 1; finale\r\n\r\n"); - fDistanzaPercorsa = 980.20; - nDistanzaPercorsa = fDistanzaPercorsa*100; // considera fino ai centesimi della distanza percorsa - - // inizializza caDistanzaPercorsa[] - for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++) - { - caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0'; - } - - // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale - nIndexDistanzaPercorsa = 0; - do - { - // generate digits in reverse order - caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0'; // get next digit - - //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica - nIndexDistanzaPercorsa++; - } - while ((nDistanzaPercorsa /= 10) > 0) ; - - // invia la distana percorsa al PC - pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c, \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica - - // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,..........................................................................................................,0x04,Data2,..............,0x05,Data3,...................,ETX; - //myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','0','0', ',','0',0x00,0x00,0x00,0x00,0x00,0x00, 0x03); - myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03); - while(true); - */ - //+++++++++++++++ Fine Test di calcolo e trasmissione distanza percorsa ++++++++++++++++++++++++ - - - - - //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++ - /* - while(true) - { - if(cCommandBLE != cOldCommandBLE) - { - - switch (cCommandBLE) - { - case 'A': - { - myLed = 1; - }; break; - case 'B': - { - myLed = 0; - }; break; - case 'C': - { - Light = 1; - }; break; - case 'D': - { - Light = 0; - }; break; - default: break; - } - // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica - cOldCommandBLE = cCommandBLE; - } - - } // while(true) Test comandi da BLE - */ - //++++++++++++ FINE ricezione comandi BLE ++++++++++++ - - //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++ - /* - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - - fDutyCycle = 0.0; - - // inizializza il pin PWM - //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore - //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore - //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore - //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore - while(true) - { - // Vai avanti Anteriore - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 1; - AntOutBI2 = 0; - PostOutPWB.write(0.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Avanti Anteriore\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai Indietro Anteriore - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 1; - PostOutPWB.write(0.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Indietro Anteriore \r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai avanti Posteriore - PostOutBI1 = 1; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(0.0); // duty cycle del PWM Anteriore - pc.printf("Avanti Posteriore\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai Indietro Posteriore - PostOutBI1 = 0; - PostOutBI2 = 1; - AntOutBI1 = 0; - AntOutBI2 = 0; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(0.0); // duty cycle del PWM Anteriore - pc.printf("Indietro Posteriore \r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai avanti Anteriore + Posteriore - PostOutBI1 = 1; - PostOutBI2 = 0; - AntOutBI1 = 1; - AntOutBI2 = 0; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Avanti Anteriore + Posteriore\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai Indietro Anteriore + Posteriore - PostOutBI1 = 0; - PostOutBI2 = 1; - AntOutBI1 = 0; - AntOutBI2 = 1; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Indietro Anteriore + Posteriore \r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai avanti Anteriore + Posteriore velocità ridotta - PostOutBI1 = 1; - PostOutBI2 = 0; - AntOutBI1 = 1; - AntOutBI2 = 0; - PostOutPWB.write(0.5); // duty cycle del PWM Posteriore - AntOutPWB.write(0.5); // duty cycle del PWM Anteriore - pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Vai Indietro Anteriore + Posteriore velocità ridotta - PostOutBI1 = 0; - PostOutBI2 = 1; - AntOutBI1 = 0; - AntOutBI2 = 1; - PostOutPWB.write(0.5); // duty cycle del PWM Posteriore - AntOutPWB.write(0.5); // duty cycle del PWM Anteriore - pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Ruota a destra - PostOutBI1 = 0; - PostOutBI2 = 1; - AntOutBI1 = 1; - AntOutBI2 = 0; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Ruota a Destra\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - // Ruota a sinistra - PostOutBI1 = 1; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 1; - PostOutPWB.write(1.0); // duty cycle del PWM Posteriore - AntOutPWB.write(1.0); // duty cycle del PWM Anteriore - pc.printf("Ruota a Sinistra\r\n"); - wait (2); - - // spegni - PostOutBI1 = 0; - PostOutBI2 = 0; - AntOutBI1 = 0; - AntOutBI2 = 0; - pc.printf("Fermo\r\n\r\n"); - wait (3); - - } // while(true) Test motore con PWM - */ - //++++++++++++ FINE Test Motore con PWM ++++++++++++ - - //++++++++++++++++++++++++++++++++++++++++++++++++++ - //++++++++++++++ FINE Ciclo test +++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++ - -} // main()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SITAEL-Comm.cpp Wed May 18 14:29:48 2022 +0000 @@ -0,0 +1,132 @@ +#include "mbed.h" +#include<stdlib.h> + +// dimensione massima del pacchetto ricevuto su seriale +#define PACKETDIM 512 + + +// Definizione periferica USB seriale del PC +Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 + +// Definizione periferica seriale del Modulo Seriale HC05 +Serial mySerial(PA_9, PA_10, 115200); //Tx, Rx, bps + + +// User Button, LED +DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 +DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5 + + + +// array per la ricezione dei messaggi da Seriale +volatile char caRxPacket[PACKETDIM]; + +// contatore e indice di caratteri ricevuti da Seriale +volatile char nRxCharCount; // Contatore dei caratteri ricevuti; Si aggiornerà modulo 256 a buffer circolare +volatile char nRxCharIndex; // Indice dell'array dei caratteri ricevuti e già visualizzati sui LED; Si aggiornerà modulo 256 a buffer circolare + +/**********************************************/ +// IRQ associata a Rx da PC +//**********************************************/ +void pcRxInterrupt(void) +{ + // array per la ricezione dei messaggi da seriale + char cReadChar; + + // ricevi caratteri su seriale, se disponibili + while((pc.readable())) + { + // acquisice stringa in input e relativa dimensione + cReadChar = pc.getc(); // read character from PC + mySerial.putc(cReadChar); // Diagnostica: write char to Seriale + pc.putc(cReadChar); // Diagnostica: write char to PC + + //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica + } +} + +//**********************************************/ +// IRQ associata a Rx da Seriale +//**********************************************/ +void SerialeRxInterrupt(void) +{ + + // carattere ricevuto da Seriale + char cReadChar; + + while((mySerial.readable())) + { + // acquisice stringa in input e memorizza in array + cReadChar = mySerial.getc(); // Read character + nRxCharCount++; // incrementa il contatore dei caratteri ricevuti + caRxPacket[nRxCharCount]=cReadChar; + + // visualizza l'ultimo carattere ricevuto + pc.printf("> Rx: %c \r\n",cReadChar); // diagnostica + } + } + +/**********/ +/* MAIN */ +/**********/ +int main() +{ + // indice per i cicli + int nIndex; + + // LED su scheda inizialmente spento + myLed = 0; + + + // messaggio di benvenuto + pc.printf("\r\n***** Hallo ***** \r\n"); + + + // inizializza contatore e indice di caratteri ricevuti + nRxCharCount = 0; + nRxCharIndex =0; + + // inizializza array di caratteri ricevuti + for(nIndex=0; nIndex<PACKETDIM; nIndex++) + { + caRxPacket[nIndex]=0; + } + + // Attiva la IRQ per la RX su seriale + mySerial.attach(&SerialeRxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del Seriale + pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ INIZIO Test +++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ FINE Test +++++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + while(true) + { + //++++++++++++++++++++++++++ INIZIO Se premuto il myButton effettua un test +++++++++++++++++++++++++++++ + if(myButton == 0) + { + myLed =1; + } + else + { + myLed =0; + } + //++++++++++++++++++++++++++ FINE Se premuto il myButton effettua test +++++++++++++++++++++++++++++ + + } //while(true) Ciclo principale + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ FINE Ciclo Principale +++++++++++++++++++ + //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +} // main()