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