Amaldi
app nuova
Dependencies: mbed
Diff: MicRobot-Rev084.cpp
- Revision:
- 17:e74856e7a232
- Parent:
- 16:2581bc608372
--- a/MicRobot-Rev084.cpp Mon Mar 02 12:47:34 2020 +0000
+++ b/MicRobot-Rev084.cpp Wed Jul 08 10:14:02 2020 +0000
@@ -4,7 +4,6 @@
#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
@@ -225,146 +224,108 @@
cReadChar = myBLE.getc(); // Read character
caRxPacket[nCharCount]=cReadChar;
nCharCount++;
- //pc.printf("%c", cReadChar); // diagnostica
+ pc.printf("%c", cReadChar); // diagnostica
// inizializza flag di incongruenza
bIncongruenza = false;
-
- if(cReadChar==')')
+ if(caRxPacket[0]=='(')
{
- //pc.printf("\r\n"); // diagnostica
+ cReadChar = myBLE.getc(); // Read character
+ caRxPacket[nCharCount]=cReadChar;
+ nCharCount++;
+ pc.printf("%c", cReadChar); // diagnostica
// +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
// Ho ricevuto il comando da un Button se il carattere numero 1, è una lettera maiuscola
- if((caRxPacket[1] > 0x40) && (caRxPacket[1] < 0x5B)) // caratteri alfabetici
+ if((caRxPacket[1] > 0x40) && (caRxPacket[1] < 0x4E)) // caratteri alfabetici
{
cCommandBLE = caRxPacket[1]; // legge e memorizza il primo carattere
nParamBLE = caRxPacket[2]-0x30;
// 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
}
// +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
- // ++++++++++++++++++ INIZIO Estrai coordinate polari del joystick +++++++++++++++++++++
-
- // protezione da potenziali errori di comunicazione. Se il numero di caratteri è maggiore di 0 non modificare i vecchi nRo e nTeta
- if(nCharCount <= 7) // un comando corretto può essere '(' '^'/'~' '-' , n, n, n ')'
+ if(cReadChar=='X')
{
- // esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
- fEsponente = 1.0;
- if(caRxPacket[1] == '~') // ricevuta 0x7E = '~', cioè ricevuto fase dal joystick
+ nIndex = 0;
+ fX=0;
+ do
{
- // stampa carattere ricevuto
- //pc.printf("Fase: '~' \n\r"); // diagnostica
- // trasforma in numero i caratteri della fase
- nTeta=0;
- for(nCharIndex = (nCharCount-2); nCharIndex > 1; nCharIndex--) // I primi due caratteri sono i delimitatori " (~ " e l'ultimo è un delimitatore ')'
- {
- nAux = caRxPacket[nCharIndex]-0x30;
- if(( nAux > 9) || (nAux < 0))
- {
- // c'è un errore. esce dal for e attiva il flag di incongruenza
- //pc.printf(">---------------------- ERRORE nAux - nTeta !!! ------------------- \r\n\r"); // diagnostica
- //nTeta=0; // imposta Teta a 0
- bIncongruenza = true; // attiva flag di incongruenza a causa di un errore di comunicazione
- break; //esce dal for
- }
- nTeta = nTeta + nAux*fEsponente;
- fEsponente*=10.0;
- //pc.printf("cReadCharacter: %c\n\r", caRxPacket[nCharIndex]); // diagnostica
- }
- // protezione da errori di trasmissione
- if ((nTeta > 360 ) || (nTeta < 0))
- {
- // c'è un errore. esce dal for e imposta nTeta =0
- //pc.printf(">---------------------- ERRORE nTeta [0 .. 365] !!! ------------------- \r\n\r"); // diagnostica
- //nTeta=0; // imposta Teta a 0
- bIncongruenza = true; // attiva flag di incongruenza a causa di un errore di comunicazione
- }
- // visualizza valore di angolo ricevuto da BLE
- //pc.printf("> nTeta = %d \n\r",nTeta); // diagnostica
- // visualizza gli ultimi valori di modulo e fase ricevuti da BLE
- //pc.printf("> (nRo,nTeta) = (%d,%d) \n\r\n\r",nRo, nTeta); // diagnostica
+ cReadChar = myBLE.getc(); // Read character
+ caRxPacket[nCharCount]=cReadChar;
+ nCharCount++;
+ pc.printf("%c", cReadChar); // diagnostica
+
+ fX = fX+ double(cReadChar - 0x30), nIndex;
+ nIndex++;
}
- // esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
- fEsponente = 1.0;
- if (caRxPacket[1] == '^') // ricevuta 0x7E = '^', cioè ricevuto modulo dal josystick
+ while((cReadChar >= 0x30) && (cReadChar <= 0x39));
+
+
+ if(cReadChar=='Y')
{
- // stampa carattere ricevuto
- //pc.printf("Modulo: '^' \n\r"); // diagnostica
- // trasforma in numero i caratteri del modulo
- nRo=0;
- for(nCharIndex = (nCharCount-2); nCharIndex > 1; nCharIndex--) // I primi due caratteri sono i delimitatori " (^ " e l'ultimo è un delimitatore ')'
+ nIndex = 0;
+ fX=0;
+ do
{
- nAux = caRxPacket[nCharIndex]-0x30;
- if((nAux > 9) || (nAux < 0))
- {
- // c'è un errore. esce dal for e imposta nTeta =0
- //pc.printf(">---------------------- ERRORE nRo [-100 .. 100] !!! ------------------- \r\n\r"); // diagnostica
- //nRo=0; // imposta Ro a 0
- bIncongruenza = true; // attiva flag di incongruenza a causa di un errore di comunicazione
- break; //esce dal for
- }
- nRo = nRo + (caRxPacket[nCharIndex]-0x30)*fEsponente; // l'ultimo carattere ricevuto è un delimitatore
- fEsponente*=10.0;
- //pc.printf("nRo provvisorio: %d\n\r", nRo); // diagnostica
- //pc.printf("cReadCharacter: %c\n\r", caRxPacket[nCharIndex]); // diagnostica
- }
- // protezione da errori di trasmissione
- if ((nRo > 100 ) || (nRo < -100))
- {
- // c'è un errore. esce dal for e imposta nRo =0
- //pc.printf(">---------------------- ERRORE nAux - nRo !!! ------------------- \r\n\r"); // diagnostica
- //nRo=0; // imposta Ro a 0
- bIncongruenza = true; // attiva flag di incongruenza a causa di un errore di comunicazione
- }
- // visualizza il valore di modulo ricevuto da BLE
- //pc.printf("> nRo = %d \n\r",nRo); // diagnostica
- // visualizza gli ultimi valori di modulo e fase ricevuti da BLE
- //pc.printf("> (nRo,nTeta) = (%d,%d) \n\r\n\r",nRo, nTeta); // diagnostica
+ cReadChar = myBLE.getc(); // Read character
+ caRxPacket[nCharCount]=cReadChar;
+ nCharCount++;
+ pc.printf("%c", cReadChar); // diagnostica
+ fY = fY+ double(cReadChar - 0x30), nIndex;
+ nIndex++;
+ }
+ while((cReadChar >= 0x30) && (cReadChar <= 0x39));
}
- } // if(nCharCount < 7)
- else
- {
- //pc.printf(">---------------------- ERRORE n > 7 !!! ------------------- \r\n\r"); // diagnostica
+ else
+ {
+ pc.printf("%c", cReadChar); // diagnostica
+ }
}
- // posizione di comodo: il joystick mantiene Teta diverso da 0 anche quando il Ro = 0. Fisicamente quest non ha senso.
- if(nRo==0)
+ }
+ else
+ {
+ pc.printf("%c", cReadChar); // diagnostica
+ }
+ if(cReadChar==')')
+ {
+ pc.printf("tutto bene. ricevuto > %c%d \r\n\r"); // diagnostica
+ }
+
+
+
+
+
+ //+++++++++++++++++++ INIZIO converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
+ // se il sw è resettato, restituisci sempre posizioni fX=0, fY=0.
+ if(!bReset)
+ {
+ // Solo in caso di congruenza cambia i valori di fX e fY. In caso di incongruenza, mantieni i valori precedenti
+ if(!bIncongruenza)
{
- nTeta = 0; // se Ro = 0, Teta deve essere =0
+ //fX = double(nRo)*cos((double)nTeta*((double)PI/180.0));
+ fX = fX-100;
+ //fY = double(nRo)*sin((double)nTeta*((double)PI/180.0));
+ fY = -fY;
+ fY = fY+100;
}
-
- // ++++++++++++++++++ FINE Estrai coordinate polari del joystick +++++++++++++++++++++
+ //pc.printf("> (fX,fY) = (%.2f,%.2f) \n\r\n\r",fX, fY); // diagnostica
+ }
+ else
+ {
+ // in caso di reset mantieni fermi i motori anche se sulla APP il joystick si sta muovendo
+ fX = 0.0;
+ fY = 0.0;
+ }
+ //+++++++++++++++++++ FINE converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
+
+ // reinizializza contatore di caratteri ricevuti
+ nCharCount = 0;
- // visualizza gli ultimi valori di modulo e fase ricevuti da BLE
- //pc.printf("> (nRo,nTeta) = (%d,%d) \n\r\n\r",nRo, nTeta); // diagnostica
-
- //+++++++++++++++++++ INIZIO converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
- // se il sw è resettato, restituisci sempre posizioni fX=0, fY=0.
- if(!bReset)
- {
- // Solo in caso di congruenza cambia i valori di fX e fY. In caso di incongruenza, mantieni i valori precedenti
- if(!bIncongruenza)
- {
- fX = double(nRo)*cos((double)nTeta*((double)PI/180.0));
- fY = double(nRo)*sin((double)nTeta*((double)PI/180.0));
- }
- //pc.printf("> (fX,fY) = (%.2f,%.2f) \n\r\n\r",fX, fY); // diagnostica
- }
- else
- {
- // in caso di reset mantieni fermi i motori anche se sulla APP il joystick si sta muovendo
- fX = 0.0;
- fY = 0.0;
- }
- //+++++++++++++++++++ FINE converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
-
- // reinizializza contatore di caratteri ricevuti
- nCharCount = 0;
-
- // comunica al cellulare vleocità e spostamento mentre si sta spostando
- //myBLE.printf(" Speed= %d [m/s]; Trip= d [m]\n\r",nRo, nTeta );
- } // if(cReadChar == ')')
+ // comunica al cellulare vleocità e spostamento mentre si sta spostando
+ //myBLE.printf(" Speed= %d [m/s]; Trip= d [m]\n\r",nRo, nTeta );
+
}
}