Amaldi
/
Amaldi_MicRobot-Rev089_1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Revision 22:cd1cd7259989, committed 2020-09-18
- Comitter:
- maristella
- Date:
- Fri Sep 18 09:28:14 2020 +0000
- Parent:
- 21:2dd20322f02b
- Commit message:
- ultima versione
Changed in this revision
| MicRobot-Rev089.cpp | Show diff for this revision Revisions of this file |
| MicRobot-Rev089_1.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2dd20322f02b -r cd1cd7259989 MicRobot-Rev089.cpp
--- a/MicRobot-Rev089.cpp Sat Sep 05 16:25:42 2020 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,781 +0,0 @@
-// pilotaggio carrello tramite BLE.
-// testato su L476RG e F401RE
-
-#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
-
-// Definizione periferica seriale del Modulo BLE ELETT114A
-Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps // F401
-//Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
-
-// 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.
-
-// 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_8); // 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
-DigitalOut WatchDog (PB_0); // output WatchDog che deve oscillare con un periodo t < 1.5 s
-//Yes+++PwmOut MotoreCoda (PB_8); // Output movimento coda
-PwmOut MotoreCoda (PA_1); // Output movimento coda
-
-DigitalInOut SwitchRouter (PB_9, PIN_OUTPUT, OpenDrain, 0); //Uscita opendrain No Pull
-
-
-//carattere di comando ricevuto dal BLE e relativo parametro
-volatile char cCommandBLE; // cambia nella routine di interrupt
-volatile char cParamBLE; // cambia nella routine di interrupt
-volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
-
-// memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
-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;
-
-// esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
-double fEsponente;
-
-// array per la ricezione dei messaggi da BLE
-volatile char caRxPacket[PACKETDIM];
-// contatore di caratteri ricevuti daBLE
-volatile int nCharCount;
-
-// 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
-//**********************************************/
-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:
- // Invia Stringa di comando al Robot
- myBLE.printf("*W\r\n> Prova di Trasmissione \r\n*");
- }
- }
-}
-
-//**********************************************/
-// IRQ associata a Rx da BLE
-//**********************************************/
-void BLERxInterrupt(void)
-{
-
- // carattere ricevuto da BLE
- char cReadChar;
-
- while((myBLE.readable()))
- {
- // acquisice stringa in input e memorizza in array
- cReadChar = myBLE.getc(); // Read character
- //caRxPacket[nCharCount]=cReadChar;
- //nCharCount++;
- // 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
-
- // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
- // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T
- if((cReadChar > 0x40) && (cReadChar < 0x55)) // caratteri alfabetici da 'A' a 'T'
- {
- // memorizza come comando il carattere appena letto
- cCommandBLE = cReadChar;
- // legge e memorizza come paramentro il successivo carattere
- cReadChar = myBLE.getc(); // legge parametro
- // pc.printf("%c", cReadChar); // diagnostica
- nParamBLE = cReadChar-0x30;
- cReadChar = myBLE.getc(); // legge la ')' di chiusura comando
- //pc.printf("%c", cReadChar); // diagnostica
-
- // visualizza comando e parametro inviato da BLE
- // pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // 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 1 invece di ) \r\n\n"); // diagnostica
- }
- }
- else // dopo la X e i numeri mi aspetto Y
- {
- pc.printf("> Errore 2 invece di Y \r\n\n"); // diagnostica
- }
- }
- else // dopo la ( mi aspetto Y
- {
- pc.printf("> Errore 3 invece di X \r\n\n"); // diagnostica
- }
- } // if(comandi alfanumerici)
- } // if(cReadChar == '(')
- }
-}
-
-/*********************************************************************************************************************************************/
-/* 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("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa ); //*W=carattere di riconoscimento per comunicazione con APP (inviare messaggio)
- }
- // stimola il watchdog esterno facendo oscillare la linea WatchDog
- if(WatchDog == 0)
- {
- WatchDog = 1;
- // pc.printf("WatchDog = 1\n\r");
- }
- else
- {
- WatchDog = 0;
- // pc.printf("WatchDog = 0\n\r");
- }
- //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
-}
-
-
-
-/**********/
-/* MAIN */
-/**********/
-int main()
-{
-
- // messaggio di benvenuto
- pc.printf("\r\n************ Hallo ****************** \r\n");
- pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
- // myBLE.printf("*W \r\nHallo\r\n*");
- //myBLE.printf("*W Modulo di Ispezione Condutture \r\n*");
-
- // inizializza PWM del motore coda
- MotoreCoda.period_ms(50); // periodo PWM
- bCodaInMovimento = false;
- MotoreCoda.write (0.0);
- // inizializza Opendrain
- SwitchRouter.mode (PullNone);
-
- // inizializza variabili da BLE
- cCommandBLE = 0; // inizialmente nessun comando da BLE
- cOldCommandBLE = 0; // inizialmente nessun comando da BLE
- 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
- WatchDog = 0; // inizialmente watchdog = 0; oscillerà per non ricevere reset esterno
-
- // 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 Raw Test Motore ++++++++++++
-
- while(1)
- {
- //if(myButton == 0)
- {
- //Yes+++MotoreCoda.write (1);
- // pc.printf("*** Run TAIL ***\r\n");
- //wait (1);
- //Yes+++MotoreCoda.write (0.0);
- //pc.printf("*** Stop RAIL ***\r\n");
- //wait (1);
- pc.printf("*** Run TAIL ***\r\n");
- MotoreCoda.write (1.0);
-
- // CW
- PostOutPWB = 1;
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- pc.printf("Run CW\r\n\r\n");
- //Yes+++MotoreCoda.write (1);
-
- wait (3);
-
-
- // spegni
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Stop CW\r\n\r\n");
- wait (3);
-
-
- // CCW
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutPWB = 1;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- pc.printf("Run CCW\r\n\r\n");
- wait (3);
-
- // spegni
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- //Yes+++MotoreCoda.write (0);
- //pc.printf("*** Stop TAIL ***\r\n");
- pc.printf("Stop CCW\r\n\r\n");
- wait (3);
-
- }
- } // while(true) Raw test motore
-
- //++++++++++++++++++++++++++++++ FINE Test Motore Raw ++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++++++++++++++++++ INIZIO Test WatchDog +++++++++++++++++++++++++++++++++++++++++
- /* while(true)
- {
- if(WatchDog == 0)
- {
- WatchDog = 1;
- pc.printf("WatchDog = 1\n\r");
- }
- else
- {
- WatchDog = 0;
- pc.printf("WatchDog = 0\n\r");
- }
-
- wait_ms(100);
-
- }
- */
- //++++++++++++++++++++++++++++++ FINE Test WatchDog +++++++++++++++++++++++++++++++++++++++++
-
- //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
- //pc.printf("Coda in movimento \n\r"); //diagnostica
- /*
- while(1)
- {
- MotoreCoda.write (1.0);
- pc.printf("*** Run TAIL ***\r\n");
- wait (2);
- MotoreCoda.write (0.0);
- pc.printf("*** Stop RAIL ***\r\n");
- wait (2);
- }
- */
- //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++ INIZIO Test Segnali ++++++++++++
- /*
- while(1)
- {
- //if(myButton == 0)
- {
- // CW
- //PostOutPWB = 1;
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- pc.printf("Run CW\r\n\r\n");
- wait (3);
-
-
- // spegni
- //PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- pc.printf("Stop CW\r\n\r\n");
- wait (3);
-
-
- // CCW
- //PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- pc.printf("Run CCW\r\n\r\n");
- wait (3);
-
- // spegni
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- pc.printf("Run CCW\r\n\r\n");
- wait (1);
- }
- } // while(true) Raw test motore
- */
- //++++++++++++++++ FINE TEST Segnali +++++++++++++++++++++++++++++++
-
-
-
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
- while(true)
- {
- 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
- //Yes+++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
- //Yes+++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);
-
- }
- }
- //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
- if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
- {
- switch (cCommandBLE)
- {
- case 'T': // accendi/spegni LED su scheda
- {
- myLed = nParamBLE;
- } break;
- case 'C': // accendi/spegni LED su scheda
- {
- if (nParamBLE == 1)
- {
- SwitchRouter = 1;
- // pc.printf("acceso\n\r");
- }
- else
- {
- SwitchRouter = 0;
- // pc.printf("spento\n\r");
- }
- } break;
- case 'L': // Accendi/spegni illuminazione a LED
- {
- Light = nParamBLE;
- } break;
- case 'R': // Reset odometria e illuminazione
- {
- if(nParamBLE==1)
- {
- bReset = true;
- nCountRiseEdge = 0;
- nOldCountRiseEdge = 0;
- Light = 0;
- fDistanzaPercorsa = 0.0;
- fSpeed = 0.0;
- }
- else
- {
- // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
- bReset = false;
- }
- } break;
-
- default: break;
- }
- //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
- cOldCommandBLE = cCommandBLE;
- nOldParamBLE = nParamBLE;
- }
-
- //++++++++++++++++++++++++++++++++++++++++++++ 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("*WSpeed= %.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
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ FINE Ciclo Principale +++++++++++++++++++
- //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-} // main()
diff -r 2dd20322f02b -r cd1cd7259989 MicRobot-Rev089_1.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MicRobot-Rev089_1.cpp Fri Sep 18 09:28:14 2020 +0000
@@ -0,0 +1,786 @@
+
+// pilotaggio carrello tramite BLE.
+// testato su L476RG e F401RE
+
+#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.05)
+
+// numero di impulsi per giro generati dall'encoder
+#define IMPULSIPERGIRO 3
+
+// 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
+
+// Definizione periferica seriale del Modulo BLE ELETT114A
+Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps // F401
+//Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
+
+// 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.
+
+// 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_7); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
+PwmOut PostOutPWB (PB_5); // 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_8); // 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_6); // Output per pilotaggio input PWM del motore B Anteriore
+//DigitalOut AntOutPWB (PB_3); // 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
+DigitalOut WatchDog (PB_0); // output WatchDog che deve oscillare con un periodo t < 1.5 s
+//Yes+++PwmOut MotoreCoda (PA_1); // Output movimento coda
+PwmOut MotoreCoda (PA_1); // Output movimento coda
+
+DigitalInOut SwitchRouter (PB_9, PIN_OUTPUT, OpenDrain, 0); //Uscita opendrain No Pull
+
+
+//carattere di comando ricevuto dal BLE e relativo parametro
+volatile char cCommandBLE; // cambia nella routine di interrupt
+volatile char cParamBLE; // cambia nella routine di interrupt
+volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
+
+// memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
+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;
+
+// esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
+double fEsponente;
+
+// array per la ricezione dei messaggi da BLE
+volatile char caRxPacket[PACKETDIM];
+// contatore di caratteri ricevuti daBLE
+volatile int nCharCount;
+
+// 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
+//**********************************************/
+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:
+ // Invia Stringa di comando al Robot
+ myBLE.printf("*W\r\n> Prova di Trasmissione \r\n*");
+ }
+ }
+}
+
+//**********************************************/
+// IRQ associata a Rx da BLE
+//**********************************************/
+void BLERxInterrupt(void)
+{
+
+ // carattere ricevuto da BLE
+ char cReadChar;
+
+ while((myBLE.readable()))
+ {
+ // acquisice stringa in input e memorizza in array
+ cReadChar = myBLE.getc(); // Read character
+ //caRxPacket[nCharCount]=cReadChar;
+ //nCharCount++;
+ // 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
+
+ // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
+ // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T
+ if((cReadChar > 0x40) && (cReadChar < 0x55)) // caratteri alfabetici da 'A' a 'T'
+ {
+ // memorizza come comando il carattere appena letto
+ cCommandBLE = cReadChar;
+ // legge e memorizza come paramentro il successivo carattere
+ cReadChar = myBLE.getc(); // legge parametro
+ // pc.printf("%c", cReadChar); // diagnostica
+ nParamBLE = cReadChar-0x30;
+ cReadChar = myBLE.getc(); // legge la ')' di chiusura comando
+ //pc.printf("%c", cReadChar); // diagnostica
+
+ // visualizza comando e parametro inviato da BLE
+ // pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // 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 1 invece di ) \r\n\n"); // diagnostica
+ }
+ }
+ else // dopo la X e i numeri mi aspetto Y
+ {
+ pc.printf("> Errore 2 invece di Y \r\n\n"); // diagnostica
+ }
+ }
+ else // dopo la ( mi aspetto Y
+ {
+ pc.printf("> Errore 3 invece di X \r\n\n"); // diagnostica
+ }
+ } // if(comandi alfanumerici)
+ } // if(cReadChar == '(')
+ }
+}
+
+/*********************************************************************************************************************************************/
+/* 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/100;
+
+ // 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)/100;
+
+ // 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("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa ); //*W=carattere di riconoscimento per comunicazione con APP (inviare messaggio)
+ }
+ // stimola il watchdog esterno facendo oscillare la linea WatchDog
+ if(WatchDog == 0)
+ {
+ WatchDog = 1;
+ // pc.printf("WatchDog = 1\n\r");
+ }
+ else
+ {
+ WatchDog = 0;
+ // pc.printf("WatchDog = 0\n\r");
+ }
+ //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
+}
+
+
+
+/**********/
+/* MAIN */
+/**********/
+int main()
+{
+
+ // messaggio di benvenuto
+ pc.printf("\r\n************ Hallo ****************** \r\n");
+ pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
+ // myBLE.printf("*W \r\nHallo\r\n*");
+ //myBLE.printf("*W Modulo di Ispezione Condutture \r\n*");
+
+ // inizializza PWM del motore coda
+ MotoreCoda.period_ms(50); // periodo PWM
+ bCodaInMovimento = false;
+ MotoreCoda.write (0.0);
+ // inizializza Opendrain
+
+ SwitchRouter.mode (PullNone);
+
+ // inizializza variabili da BLE
+ cCommandBLE = 0; // inizialmente nessun comando da BLE
+ cOldCommandBLE = 0; // inizialmente nessun comando da BLE
+ 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
+ WatchDog = 0; // inizialmente watchdog = 0; oscillerà per non ricevere reset esterno
+
+ // 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 Raw Test Motore ++++++++++++
+ /*
+ while(1)
+ {
+ //if(myButton == 0)
+ {
+ //Yes+++MotoreCoda.write (1);
+ // pc.printf("*** Run TAIL ***\r\n");
+ //wait (1);
+ //Yes+++MotoreCoda.write (0.0);
+ //pc.printf("*** Stop RAIL ***\r\n");
+ //wait (1);
+ pc.printf("*** Run TAIL ***\r\n");
+ // MotoreCoda.write (1.0);
+
+ // CW
+ PostOutPWB = 1;
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ AntOutPWB = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+ pc.printf("Run CW\r\n\r\n");
+ //Yes+++MotoreCoda.write (1);
+
+ wait (3);
+
+
+ // spegni
+ PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutPWB = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Stop CW\r\n\r\n");
+ wait (3);
+
+
+ // CCW
+ PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ AntOutPWB = 1;
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ pc.printf("Run CCW\r\n\r\n");
+ wait (3);
+
+ // spegni
+ PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutPWB = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ //Yes+++MotoreCoda.write (0);
+ //pc.printf("*** Stop TAIL ***\r\n");
+ pc.printf("Stop CCW\r\n\r\n");
+ wait (3);
+
+
+ }
+ } // while(true) Raw test motore
+ */
+ //++++++++++++++++++++++++++++++ FINE Test Motore Raw ++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++++++++++++++++++ INIZIO Test WatchDog +++++++++++++++++++++++++++++++++++++++++
+ /* while(true)
+ {
+ if(WatchDog == 0)
+ {
+ WatchDog = 1;
+ pc.printf("WatchDog = 1\n\r");
+ }
+ else
+ {
+ WatchDog = 0;
+ pc.printf("WatchDog = 0\n\r");
+ }
+
+ wait_ms(100);
+
+ }
+ */
+ //++++++++++++++++++++++++++++++ FINE Test WatchDog +++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
+ //pc.printf("Coda in movimento \n\r"); //diagnostica
+ /*
+ while(1)
+ {
+ MotoreCoda.write (1.0);
+ pc.printf("*** Run TAIL ***\r\n");
+ wait (2);
+ MotoreCoda.write (0.0);
+ pc.printf("*** Stop RAIL ***\r\n");
+ wait (2);
+ }
+ */
+ //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++ INIZIO Test Segnali ++++++++++++
+ /*
+ while(1)
+ {
+ //if(myButton == 0)
+ {
+ // CW
+ //PostOutPWB = 1;
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ pc.printf("Run CW\r\n\r\n");
+ wait (3);
+
+
+ // spegni
+ //PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ pc.printf("Stop CW\r\n\r\n");
+ wait (3);
+
+
+ // CCW
+ //PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ pc.printf("Run CCW\r\n\r\n");
+ wait (3);
+
+ // spegni
+ PostOutPWB = 1;
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ pc.printf("Run CCW\r\n\r\n");
+ wait (1);
+ }
+ } // while(true) Raw test motore
+ */
+ //++++++++++++++++ FINE TEST Segnali +++++++++++++++++++++++++++++++
+
+
+
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ while(true)
+ {
+ 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);
+
+ }
+ }
+ //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
+ if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
+ {
+ switch (cCommandBLE)
+ {
+ case 'T': // accendi/spegni LED su scheda
+ {
+ myLed = nParamBLE;
+ } break;
+ case 'C': // accendi/spegni LED su scheda
+ {
+ if (nParamBLE == 1)
+ {
+ SwitchRouter = 1;
+ // pc.printf("acceso\n\r");
+ }
+ else
+ {
+ SwitchRouter = 0;
+ // pc.printf("spento\n\r");
+ }
+ } break;
+ case 'L': // Accendi/spegni illuminazione a LED
+ {
+ Light = nParamBLE;
+ } break;
+ case 'R': // Reset odometria e illuminazione
+ {
+ if(nParamBLE==1)
+ {
+ bReset = true;
+ nCountRiseEdge = 0;
+ nOldCountRiseEdge = 0;
+ Light = 0;
+ fDistanzaPercorsa = 0.0;
+ fSpeed = 0.0;
+ }
+ else
+ {
+ // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
+ bReset = false;
+ }
+ } break;
+
+ default: break;
+ }
+ //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
+ cOldCommandBLE = cCommandBLE;
+ nOldParamBLE = nParamBLE;
+ }
+
+ //++++++++++++++++++++++++++++++++++++++++++++ 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("*WSpeed= %.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
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ FINE Ciclo Principale +++++++++++++++++++
+ //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+} // main()