Amaldi
Versione recente
Dependencies: mbed
Diff: MicRobot-Rev087.cpp
- Revision:
- 20:513676b984bf
- Parent:
- 19:19892c59a33b
--- a/MicRobot-Rev087.cpp Thu Jul 09 14:43:47 2020 +0000
+++ b/MicRobot-Rev087.cpp Thu Sep 03 08:20:54 2020 +0000
@@ -79,6 +79,7 @@
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
PwmOut MotoreCoda (PB_8); // Output movimento coda
+DigitalInOut SwitchRouter (PB_9, PIN_OUTPUT, OpenDrain, 0); //Uscita opendrain No Pull
//carattere di comando ricevuto dal BLE e relativo parametro
@@ -216,14 +217,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
@@ -233,13 +234,13 @@
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
}
// +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
else
@@ -253,7 +254,7 @@
cReadChar = myBLE.getc(); // Read character
caRxPacket[nCharCount]=cReadChar;
nCharCount++;
- pc.printf("%c", cReadChar); // diagnostica
+ // pc.printf("%c", cReadChar); // diagnostica
}
while((cReadChar >= 0x30) && (cReadChar <= 0x39));
//+++++++++ FINE acquisisce il valore dell'ascissa Xnnn +++++++++++++++
@@ -277,7 +278,7 @@
cReadChar = myBLE.getc(); // Read character
caRxPacket[nCharCount]=cReadChar;
nCharCount++;
- pc.printf("%c", cReadChar); // diagnostica
+ // pc.printf("%c", cReadChar); // diagnostica
}
while((cReadChar >= 0x30) && (cReadChar <= 0x39));
//+++++++++ FINE acquisisce il valore dell'ordinata Ynnn +++++++++++++++
@@ -294,27 +295,27 @@
// se riceve la coda del comando ), stampa le coordinate ricevute
if(cReadChar==')')
{
- pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica
+ // 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
+ // 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
+ pc.printf("> Errore 1 invece di ) \r\n\n"); // diagnostica
}
}
else // dopo la X e i numeri mi aspetto Y
{
- pc.printf("> Errore = %c invece di Y \r\n\n", cReadChar); // diagnostica
+ pc.printf("> Errore 2 invece di Y \r\n\n"); // diagnostica
}
}
else // dopo la ( mi aspetto Y
{
- pc.printf("> Errore = %c invece di X \r\n\n", cReadChar); // diagnostica
+ pc.printf("> Errore 3 invece di X \r\n\n"); // diagnostica
}
} // if(comandi alfanumerici)
} // if(cReadChar == '(')
@@ -368,18 +369,18 @@
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)
+ // 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");
+ // pc.printf("WatchDog = 1\n\r");
}
else
{
WatchDog = 0;
- pc.printf("WatchDog = 0\n\r");
+ // pc.printf("WatchDog = 0\n\r");
}
//++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
}
@@ -391,16 +392,20 @@
/**********/
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");
- myBLE.printf("*W \r\nHallo\r\n*");
- myBLE.printf("*W 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
@@ -480,39 +485,18 @@
//++++++++++++++++++++++++++++++ FINE Test WatchDog +++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++ 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);
-
- }
- }
-
+ /* while(true)
+ {
+ MotoreCoda.write (0.6);
+ pc.printf("*** Acceso ***\r\n");
+ wait (1);
+ MotoreCoda.write (0.0);
+ pc.printf("*** Spento ***\r\n");
+ wait (1);
}
- */
+ */
//++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
@@ -522,37 +506,34 @@
while(true)
{
-
- //++++++++++++++++++++++++++ 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 ((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)
@@ -561,17 +542,22 @@
{
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;
- /* if (nParamBLE == 1)
- {
- myBLE.printf("*lR250G0B100*");
- }
- else
- {
- myBLE.printf("*lR0G0B0*");
- }*/
} break;
case 'R': // Reset odometria e illuminazione
{
@@ -627,9 +613,8 @@
// 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 );
+ // 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
{
@@ -679,7 +664,6 @@
}
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