Space
Test Comm
Dependencies: mbed
Revision 7:7043da244e4b, committed 2019-08-08
- Comitter:
- pinofal
- Date:
- Thu Aug 08 09:04:07 2019 +0000
- Parent:
- 6:92b0e17593e0
- Child:
- 8:7a81e89f60df
- Commit message:
- Da provare
Changed in this revision
| BLE-MegaPiSea_Rev3_WIP.cpp | Show diff for this revision Revisions of this file |
| Mic-Rev03.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/BLE-MegaPiSea_Rev3_WIP.cpp Mon Aug 05 09:03:43 2019 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,947 +0,0 @@
-// bpilotaggio carrello tramite BLE.
-// testato su L476RG
-
-#include "mbed.h"
-#include<stdlib.h>
-
-// pi greco
-#define PI 3.14159265358979323846
-
-// 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, 115200); //Tx, Rx, bps
-
-// Input di Reset per il Modulo BLE ELETT114A.
-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
-
-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
-
-
-// carattere letto dalla seriale del PC
-char cReadChar;
-
-// DutyCycle del segnale PWM
-float fDutyCycle;
-
-
-// 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
-
-//DigitalInOut OutBlades (PB_9, PIN_OUTPUT, OpenDrain, 0); // Output per il pilotaggio del Relay di azionamento Lame Rotanti
-
-
-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
-
-
-
-/*
-//+++DigitalIn PostInNE1 (BHO); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
-DigitalOut PostOutBI2 (PG_15); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
-DigitalOut PostOutBI1 (PA_15); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
-//PwmOut PostOutPWB (PH_13); // Output per pilotaggio input PWM del motore B Posteriore
-DigitalOut PostOutPWB (PH_13); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore
-
-DigitalOut AntOutBI2 (PB_5); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
-DigitalOut AntOutBI1 (PA_5); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
-//PwmOut AntOutPWB (PB_4); // Output per pilotaggio input PWM del motore B Posteriore
-DigitalOut AntOutPWB (PB_4); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore
-*/
-
-
-//carattere di comando ricevuto dal BLE
-volatile char cCommandBLE; // cambia nella routine di interrupt
-// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
-char cOldCommandBLE;
-
-// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
-volatile double fTeta;
-volatile double fRo;
-
-// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
-volatile double fX, fY;
-// memorizza ultimi valori delle coordinate del joistick
-double fOldX, fOldY;
-
-// variabili ausiliarie per l'algoritmo di posizionamento
-double fV, fW;
-
-// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
-double fR, fL;
-
-
-
-
-
-
-//**********************************************/
-// IRQ associata a Rx da BLE
-//**********************************************/
-void BLERxInterrupt(void)
-{
- // array per la ricezione dei messaggi da seriale
- char cReadChar;
-
- //indice per i cicli
- int nIndex;
-
- // array per la ricezione dei messaggi da seriale
- char caRxPacket[8];
- //int nRxPacketSize;
-
- // coordinate cartesiane della posizione joystick
- //float fX, fY;
- // coordinate polari della posizione joistick
- //float fTeta;
- //float fRo;
-
-
-
- //pc.printf("BLE RxInterrupt: \n\r");
-
- // ricevi caratteri su seriale, se disponibili
- while((myBLE.readable()))
- {
- // acquisice stringa in input e relativa dimensione
- cReadChar = myBLE.getc(); // Read character
- if(cReadChar == 0x02)
- {
- pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
-
- //-- command will be 8 bytes for joystick values
- //-- command will be 3 bytes for button change event
- //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
-
-
- caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
- cReadChar = myBLE.getc(); // legge il secondo carattere
- if(cReadChar > 0x40)
- {
- // Button:
- //-- Button events send a single character in a 3-byte packet
- //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
- //-- B2 uses "C" for changed to on, "D" for changed to off
- //-- B3..B6 follow in order; valid button even characters are "A".."L"
- caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
- caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX
- // passa il comando ricevuto nella variabile globale
- cCommandBLE = caRxPacket[1];
-
- // Diagnostica
- /*
- pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
- */
-
- }
- else
- {
- // Joystick:
- //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis
- //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
- //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
- caRxPacket[1] = cReadChar; // memorizza il secondo carattere
- for(nIndex=2; nIndex<8; nIndex++)
- {
- caRxPacket[nIndex] = myBLE.getc();
- }
- // dal messaggio estrae e visualizza le coordinate cartesiane
- fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
- fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
- // converte la posizione del joistick in coordinate polari
- fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
- if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
- fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
-
- // diagnostica
- /*
- pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE
- pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
- pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
- */
- }
- }
- }
-
-}
-
-/**********************************************/
-// 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); // write char to BLE
-
- //pc.printf("W>: 0x%02x\n\r",cReadChar);
- if(cReadChar == '0') // se scrivo '0', invia questa stringa
- {
- //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
- //-- button status is binary formatted string (no indicator)
- //-- data fields sent as strings
- //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
- // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
- myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
- // diagnostica
- /*
- pc.printf("W>: Inviato comando a BLE\n\r");
- */
- }
- }
-}
-
-
-/**********/
-/* MAIN */
-/**********/
-
-int main()
-{
-
- // messaggio di benvenuto
- pc.printf("\r\n******* Hallo - Exercise ********** \r\n");
- pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n");
-
- //Inizializza pin di output per Rotating Blades
- /*
- OutBlades.mode(OpenDrain);
- OutBlades.output();
- OutBlades.write(0);
- nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente
- */
- // inizializza variabili
- cCommandBLE = 0; // inizialmente nessun comando da BLE
- cOldCommandBLE = 0; // inizialmente nessun comando da BLE
- fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
- fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
- fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
- fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
-
- // inizializza il BLE
- BleRst = 0;
- wait_ms(100);
- BleRst = 1;
- cCommandBLE = '0';
- cOldCommandBLE = '0';
-
-
-
- // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore
- PostOutPWB.period_us(10); // periodo del PWM Posteriore
- PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
- AntOutPWB.period_us(10); // periodo del PWM Anteriore
- AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
-
-
- // Attiva la IRQ per la RX su seriale
- myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
- pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ INIZIO Ciclo Principale +++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++++
-
- while(true)
- {
-
- if (InDiag == 1)
- myLed = 1 ;
- else
- myLed = 0 ;
-
- //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.
- if(cCommandBLE != cOldCommandBLE)
- {
-
- switch (cCommandBLE)
- {
- case 'A':
- {
- myLed = 1;
- }; break;
- case 'B':
- {
- myLed = 0;
- }; break;
- case 'C':
- {
- Light = 1;
- }; break;
- case 'D':
- {
- Light = 0;
- }; break;
- default: break;
- }
- // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
- cOldCommandBLE = cCommandBLE;
- }
- // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote
- if( (fX != fOldX) || (fY != fOldY))
- {
- fOldX = fX;
- fOldY = fY;
- // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
- fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
- fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
- fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
- fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
-
- // diagnostica
- pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
- pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
- pc.printf("> Velocita' Right R = %.2f\r\n", fR);
- pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL);
-
- // algoritmo di movimentazione delle ruote.
- if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
- {
- fR =-fR;
- // Vai indietro
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- }
- else
- {
-
- if(fR >0)
- {
- // Vai avanti
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- }
- else
- {
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- }
- }
- PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
- if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
- {
- fL =-fL;
- // Vai indietro
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- }
- else
- {
- if(fL >0)
- {
- // Vai avanti
- AntOutBI1 = 0;
- AntOutBI2 = 1;
-
- }
- else
- {
- // spegni
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- }
- }
- AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
- }
- } //while (true) Ciclo principale
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ FINE Ciclo Principale +++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++
-
-
-
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-
-
-
- //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++
- while(true)
- {
- if(cCommandBLE != cOldCommandBLE)
- {
-
- switch (cCommandBLE)
- {
- case 'A':
- {
- myLed = 1;
- }; break;
- case 'B':
- {
- myLed = 0;
- }; break;
- case 'C':
- {
- Light = 1;
- }; break;
- case 'D':
- {
- Light = 0;
- }; break;
- default: break;
- }
- // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
- cOldCommandBLE = cCommandBLE;
- }
-
- } // while(true) Test comandi da BLE
-
- //++++++++++++ FINE ricezione comandi BLE ++++++++++++
-
- //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
- /*
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
-
- fDutyCycle = 0.0;
-
- // inizializza il pin PWM
- //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
- //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
- //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
- //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
- while(true)
- {
- if(cCommandBLE != cOldCommandBLE)
- {
-
- if(cCommandBLE == 'A')
- {
- myLed = 1;
-
-
- }
-
- else
- {
- myLed = 0;
- }
- pc.printf("Comando = %c \r\n", cCommandBLE);
- cOldCommandBLE = cCommandBLE;
- }
- {
- // Vai avanti Anteriore
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Avanti Anteriore\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai Indietro Anteriore
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Indietro Anteriore \r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai avanti Posteriore
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
- pc.printf("Avanti Posteriore\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai Indietro Posteriore
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
- pc.printf("Indietro Posteriore \r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai avanti Anteriore + Posteriore
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Avanti Anteriore + Posteriore\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai Indietro Anteriore + Posteriore
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Indietro Anteriore + Posteriore \r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai avanti Anteriore + Posteriore velocità ridotta
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
- pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai Indietro Anteriore + Posteriore velocità ridotta
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
- pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
-
-
- // Ruota a destra
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Ruota a Destra\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Ruota a sinistra
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Ruota a Sinistra\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- }
- } // while(true) Test motore con PWM
- */
- //++++++++++++ FINE Test Motore con PWM ++++++++++++
-
- //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++
- /*
- // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
-
- fDutyCycle = 0.0;
-
- // inizializza il pin PWM
- PostOutPWB.period_us(100); // periodo del PWM Posteriore
- PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
- AntOutPWB.period_us(100); // periodo del PWM Anteriore
- AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
- while(true)
- {
- //Parsing del comando ricevuto solo se cambia il comando.
- if(cCommandBLE != cOldCommandBLE)
- {
- // ricorda il vecchio comando
- cOldCommandBLE = cCommandBLE;
- //cambia velocità del PWM
- fDutyCycle = double(cOldCommandBLE - 0x41)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
- PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore
- AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore
- pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle);
- //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM
- //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0));
-
- //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione
- switch (cOldCommandBLE) // usa cOldCommandBLE.
- {
- case 'A':
- {
- // se ricevi il comando 'A'
- // Vai avanti
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Avanti\r\n\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (1);
- }break;
- case 'B':
- {
- // se ricevi il comando 'B'
- // Vai Indietro
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Indietro\r\n\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (1);
- } break;
-
- case 'C':
- {
- // se ricevi il comando 'C'
- // Ruota a destra
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Destra\r\n\r\n");
- wait (3.0);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (1);
- } break;
- case 'D':
- {
- // se ricevi il comando 'D'
- // Ruota a sinistra
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
- pc.printf("Sinistra\r\n\r\n");
- wait (3.0);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (1);
- } break;
- case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti
- {
- // se ricevi il comando 'L' Cambia direzione di movimento delle ruote
- PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0
- AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0
-
- // inverti direzione di movimento
- PostOutBI1 = !PostOutBI1;
- PostOutBI2 = !PostOutBI2;
- AntOutBI1 = !AntOutBI1;
- AntOutBI2 = !AntOutBI2;
- // ricomincia a bassa velocità
- PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore
- AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore
- } break;
- default:
- {
- // se ricevi comandi diversi, spegni i motori
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (1);
- } break;
- }
- }
- } // while(true) Test motore con PWM pilotato da BLE
- */
- //++++++++++++ FINE Test PWM tramite BLE ++++++++++++
-
-
- /*
- //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
-
- fDutyCycle = 0.0;
-
- // inizializza il pin PWM
- PostOutPWB.period_us(100); // periodo del PWM Posteriore
- PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
- //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
- //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
- while(true)
- {
- //+++if(cCommandBLE == 'A')
- {
- // Vai avanti
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
- pc.printf("Avanti\r\n\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
- // Vai Indietro
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
- AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
- pc.printf("Indietro\r\n\r\n");
- wait (2);
-
- // spegni
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- pc.printf("Fermo\r\n\r\n");
- wait (3);
-
-
-
- }
- } // while(true) Test motore con PWM
- */
- //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++
-
-
-
- //++++++++++++ INIZIO Test Motore ++++++++++++
- /*
- while(1)
- {
- //if(myButton == 0)
- {
- // CW
- PostOutPWB = 1;
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 1;
- pc.printf("CW\r\n\r\n");
- wait (2);
-
-
- // spegni
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
- wait (1);
-
-
- // CCW
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 1;
- AntOutPWB = 1;
- AntOutBI1 = 1;
- AntOutBI2 = 0;
- wait (2);
- pc.printf("CCW\r\n\r\n");
-
- // spegni
- PostOutPWB = 1;
- PostOutBI1 = 0;
- PostOutBI2 = 0;
- AntOutPWB = 1;
- AntOutBI1 = 0;
- AntOutBI2 = 0;
-
- wait (1);
-
- }
- } // while(true) test motore
- */
- //++++++++++++ FINE Test Motore ++++++++++++
-
-
- //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++
- /*
- // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
- PostOutBI1 = 1;
- PostOutBI2 = 0;
- fDutyCycle = 0.5;
-
- // inizializza il pin PWM
- PostOutPWB.period_ms(1000); // periodo del PWM
- PostOutPWB.write(fDutyCycle); // duty cycle del PWM
- while(true)
- {
- // verifica se è arrivato un carattere dalla seriale del PC
- if(pc.readable())
- {
- cReadChar = pc.getc(); // Read hyperterminal
- fDutyCycle = float(cReadChar - 0x30)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
- pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle);
- //PostOutPWB.period_ms(1000); // periodo del PWM
- //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM
-
- // Inverti direzione del moto
- //PostOutBI1 != PostOutBI1;
- //PostOutBI2 != PostOutBI2;
-
- PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM
-
- }
- } // while(true) test PWM tramite seriale del PC
- */
- //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++
-
- //++++++++++++++++++++++++++++++++++++++++++++++++++
- //++++++++++++++ FINE Ciclo test +++++++++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++
-
-} // main()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mic-Rev03.cpp Thu Aug 08 09:04:07 2019 +0000
@@ -0,0 +1,839 @@
+// pilotaggio carrello tramite BLE.
+// testato su L476RG e F401RE
+
+#include "mbed.h"
+#include<stdlib.h>
+
+
+// pi greco
+#define PI 3.14159265358979323846
+
+// diametro della ruota in [metri]
+#define DIAMETRORUOTA (0.1)
+
+// numero di impulsi per giro generati dall'encoder
+#define IMPULSIPERGIRO 4
+
+// numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m]
+#define NUMCIFREDISTANZAPERCORSA 5
+
+// numero di cifre con cui si vuole rappresentare la velocità in [m/s]. NUMCIFRESPEED = 5, significa che la velocità è rappresentata come xxx.xx [m/s]
+#define NUMCIFRESPEED 5
+
+// intervallo di tempo in [msec], cui vengono contati gli impulsi di encoder per il calcolo della velocità
+#define DELTAT 200
+
+// ticker per il calcolo della velocità
+//Ticker SpeedTicker;
+
+// Timer per il calcolo della velocità. Nel periodo di tempo del timer, conta gli impulsi ricevuti come interrupt dall'encoder
+Timer TimerSpeed;
+
+// tempo inizio intermedio e fine del timer che misura il tempo per il calcolo della velocità
+int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;
+
+// Ticker per la simulazione di segnale proveniente da encoder sul motore
+Ticker SpeedTicker;
+
+// Definizione periferica USB seriale del PC
+Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
+
+// Definizione periferica seriale del Modulo BLE ELETT114A
+Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps
+
+// Input di Reset per il Modulo BLE HC-05
+DigitalOut BleRst(PA_8);
+
+// User Button, LED
+DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
+DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
+
+// output digitale per pilotaggio illuminazione a LED
+DigitalOut Light(PA_0);
+//DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20
+InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore.
+
+// carattere letto dalla seriale del PC
+char cReadChar;
+
+// DutyCycle del segnale PWM
+float fDutyCycle;
+
+// variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot
+volatile int nCountRiseEdge;
+volatile int nOldCountRiseEdge;
+
+// Input/Output
+DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
+PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore
+//DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
+DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
+DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
+
+DigitalOut AntOutBI1 (PB_4); // Output 1 per pilotaggio input BI1 del Motore B Anteriore
+PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore
+//DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
+DigitalOut AntOutBI2 (PB_3); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
+DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
+
+
+//carattere di comando ricevuto dal BLE
+volatile char cCommandBLE; // cambia nella routine di interrupt
+// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
+char cOldCommandBLE;
+
+// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
+volatile double fTeta;
+volatile double fRo;
+
+// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
+volatile double fX, fY;
+// memorizza ultimi valori delle coordinate del joistick
+double fOldX, fOldY;
+
+// variabili ausiliarie per l'algoritmo di posizionamento
+double fV, fW;
+
+// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
+double fR, fL;
+
+// distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore
+double fDistanzaPercorsa;
+
+// velocità calcolata gli impulsi contati in un intervallo DELTAT msec
+double fSpeed;
+
+// Scopi diagnostici: Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
+// velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+double fDeltaTick;
+
+// distanza percorsa e relativo indice, calcolata in [m] e trasformata in caratteri | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m]
+//char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
+//int nIndexDistanzaPercorsa;
+
+//**********************************************/
+// IRQ associata a Rx da BLE
+//**********************************************/
+void BLERxInterrupt(void)
+{
+ // array per la ricezione dei messaggi da seriale
+ char cReadChar;
+
+ //indice per i cicli
+ int nIndex;
+
+ // array per la ricezione dei messaggi da seriale
+ char caRxPacket[8];
+ //int nRxPacketSize;
+
+ // variabile per estrarre le cifre della distanza percorsa. La distanza Percorsa viene calcolata nel MAIN con la varibile fDistanzaPercorsa
+ int nDistanzaPercorsa;
+ // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m]
+ char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
+ int nIndexDistanzaPercorsa;
+
+ // variabile per estrarre le cifre della velocità di percorrenza. La velocità viene calcolata nel MAIN con la varibile fSpeed
+ int nSpeed;
+ // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice | centinaia di [m] | decine di [m] | [m] | decimi di [m] | centesimi di [m]
+ char caSpeed[NUMCIFRESPEED];
+ int nIndexSpeed;
+
+
+ //pc.printf("BLE RxInterrupt: \n\r");
+
+ // ricevi caratteri su seriale, se disponibili
+ while((myBLE.readable()))
+ {
+ // acquisice stringa in input e relativa dimensione
+ cReadChar = myBLE.getc(); // Read character
+ if(cReadChar == 0x02)
+ {
+
+ //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
+
+ //-- command will be 8 bytes for joystick values
+ //-- command will be 3 bytes for button change event
+ //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
+
+
+ caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
+ cReadChar = myBLE.getc(); // legge il secondo carattere
+ if(cReadChar > 0x40)
+ {
+ // Button:
+ //-- Button events send a single character in a 3-byte packet
+ //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
+ //-- B2 uses "C" for changed to on, "D" for changed to off
+ //-- B3..B6 follow in order; valid button even characters are "A".."L"
+ caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
+ caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX
+ // passa il comando ricevuto nella variabile globale
+ cCommandBLE = caRxPacket[1];
+
+ // Diagnostica
+ /*
+ pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
+ */
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ }
+ else
+ {
+ //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggi Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ // Joystick:
+ //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis
+ //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
+ //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
+ caRxPacket[1] = cReadChar; // memorizza il secondo carattere
+ for(nIndex=2; nIndex<8; nIndex++)
+ {
+ caRxPacket[nIndex] = myBLE.getc();
+ }
+ // dal messaggio estrae e visualizza le coordinate cartesiane
+ fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
+ fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
+ // converte la posizione del joistick in coordinate polari
+ fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
+ if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
+ fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
+
+ // diagnostica
+ /*
+ pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE
+ pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
+ pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
+ */
+ }
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //fDistanzaPercorsa = 980.20; // diagnostica
+ nDistanzaPercorsa = fDistanzaPercorsa*100; // moltiplica per 100 per considerare fino ai centesimi della distanza percorsa
+ nSpeed = fSpeed*100; // moltiplica per 100 per considerare fino ai centesimi della velocità
+
+ // inizializza caDistanzaPercorsa[]
+ for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
+ {
+ caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
+ }
+
+ // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
+ nIndexDistanzaPercorsa = 0;
+ do
+ {
+ // genera digits della distanza percorsa
+ caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0';
+
+ //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica
+ nIndexDistanzaPercorsa++;
+ }
+ while ((nDistanzaPercorsa /= 10) > 0) ;
+ // Diagnostica: visualizza la distanza percorsa sul PC
+ //pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ // inizializza caSpeed[]
+ for(nIndexSpeed = 0; nIndexSpeed < NUMCIFREDISTANZAPERCORSA; nIndexSpeed++)
+ {
+ caSpeed[nIndexSpeed] = '0';
+ }
+
+ // estrae le singole cifre di fSpeed, fino alla seconda cifra decimale
+ nIndexSpeed = 0;
+ do
+ {
+ // genera digits della velocità
+ caSpeed[nIndexSpeed] = nSpeed % 10 + '0';
+
+ //pc.printf("caSpeed[%d]: %c \n\r", nIndexSpeed, caSpeed[nIndexSpeed]); // diagnostica
+ nIndexSpeed++;
+ }
+ while ((nSpeed /= 10) > 0) ;
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++++++++++++++++++++++++++++++++ INIZIO Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ // Diagnostica: visualizza la velocità sul PC
+ //pc.printf("Speed: %c,%c,%c,%c,%c \n\r", caSpeed[4], caSpeed[3], caSpeed[2], caSpeed[1], caSpeed[0]); // diagnostica
+
+ // Struttura Nominale del comando da inviare al robot STX ,B6, B5, B4, B3 B2 B1 0x01,Data1,...........................................................................................................,0x04,Data2,.....................................................,0x05,Data3,...................,ETX;
+ myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,' ',' ',' ',' ',' ',' ',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,caSpeed[4],caSpeed[3],caSpeed[2], ',',caSpeed[1],caSpeed[0],0x05,0x30,0x30,0x30,0x30,0x30, 0x03);
+
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ }
+ }
+}
+
+/**********************************************/
+// IRQ associata a Rx da PC
+//**********************************************/
+void pcRxInterrupt(void)
+{
+ // array per la ricezione dei messaggi da seriale
+ char cReadChar;
+
+ // ricevi caratteri su seriale, se disponibili
+ while((pc.readable()))
+ {
+ // acquisice stringa in input e relativa dimensione
+ cReadChar = pc.getc(); // read character from PC
+ //myBLE.putc(cReadChar); // Diagnostica: write char to BLE
+ //pc.putc(cReadChar); // Diagnostica: write char to PC
+
+ //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica
+ if(cReadChar == '0') // se scrivo '0', invia questa stringa
+ {
+ // DIAGNOSTICA:
+ // pc.printf("W>: Inviato comando a BLE\n\r"); // diagnostica
+ //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
+ //-- button status is binary formatted string (no indicator)
+ //-- data fields sent as strings
+ //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
+ // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
+ myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
+ }
+ }
+}
+
+
+/**************************************************************************************/
+/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
+/**************************************************************************************/
+void riseEncoderIRQ()
+{
+ nCountRiseEdge++;
+}
+
+/****************************************************************************************/
+/* Diagnostica: */
+/* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO */
+/* Routine di gestione del ticker per simulare encoder */
+/* Simula il segnale di encoder ricevuto con un determinato DELTAT */
+/* A robot fermo, il segnale di encoder non genera interrupt. */
+/* Questo Ticker simula l'arrivo del segnale da encoder */
+/****************************************************************************************/
+void SpeedCalculate()
+{
+ // ad ogni tick viene ricevuto un impulso da encoder.
+ // velocità = (Spazio per ogni tick)/(tempo per ogni tick)
+ // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+
+ // simula impulso inviato dall'encoder
+ nCountRiseEdge++;
+}
+
+
+/**********/
+/* MAIN */
+/**********/
+int main()
+{
+
+ // messaggio di benvenuto
+ pc.printf("\r\n************ Hallo ****************** \r\n");
+ pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
+
+ // inizializza variabili da BLE
+ cCommandBLE = 0; // inizialmente nessun comando da BLE
+ cOldCommandBLE = 0; // inizialmente nessun comando da BLE
+ fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+ fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+ fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+ fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+
+ // inizializza il BLE
+ BleRst = 0;
+ wait_ms(100);
+ BleRst = 1;
+ cCommandBLE = '0';
+ cOldCommandBLE = '0';
+
+ // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore
+ PostOutPWB.period_us(10); // periodo del PWM Posteriore
+ PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
+ AntOutPWB.period_us(10); // periodo del PWM Anteriore
+ AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
+
+ // inizializza variabili
+ fDistanzaPercorsa = 0.0;
+ fSpeed = 0.0;
+
+ // Attiva la IRQ per la RX su seriale
+ myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
+ pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
+
+ // attiva un ticker per simulare robot in movimento.
+ //+++++++++ INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
+ /*
+ fDeltaTick = 0.05; // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+ SpeedTicker.attach(&SpeedCalculate,fDeltaTick); // Diagnostica
+ */
+ //+++++++++ FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
+
+ //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ // definisci il mode del segnale digitale di EncoderA
+ InEncoderA.mode(PullUp);
+
+ // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
+ InEncoderA.rise(&riseEncoderIRQ);
+
+ // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
+ nCountRiseEdge=0;
+ nOldCountRiseEdge=0;
+ //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ INIZIO Ciclo Principale +++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ while(true)
+ {
+ //+++++++++++++++++++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++++++++++++++++++++++
+
+ // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi la velocità di spostamento del robot
+ InEncoderA.enable_irq();
+
+ // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
+ TimerSpeed.start();
+ nTimerStart=TimerSpeed.read_ms();
+
+ // per 100ms conta gliimpulsi sull'encoder
+ while( (nTimerCurrent-nTimerStart) < DELTAT) // attende il passare di DELTAT millisec
+ {
+ nTimerCurrent=TimerSpeed.read_ms();
+ // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));
+ }
+ TimerSpeed.stop(); // ferma il timer
+
+ // disabilita interrupt sul segnale di encoder per contare il numero di impulsi. Disattivandolo, si evita di continuare a mandare in interrupt il processore
+ InEncoderA.enable_irq();
+ //+++++++++++++++++++++++++++++ FINE calcola spostamento con encoder sul motore ++++++++++++++++++++++++++++++++++++++++
+
+ //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++
+ //nCountRiseEdge++; //----diagnostica
+ // se nella IRQ, durante il periodo di calcolo della velocità, sono stati contati fronti di salita dell'encoder, il robot si sta muovendo
+ if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo
+ {
+ // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
+ fDistanzaPercorsa = (PI*DIAMETRORUOTA/IMPULSIPERGIRO)*nCountRiseEdge;
+
+ // calcola la velocità in [m/sec]. DELTAT è in [msec] lo spostamento è in [m]
+ fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/float(DELTAT/1000.0);
+
+ // ricorda lo spostamento
+ nOldCountRiseEdge = nCountRiseEdge;
+ }
+ //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
+ if(cCommandBLE != cOldCommandBLE)
+ {
+ switch (cCommandBLE)
+ {
+ case 'A': // accendi LED su scheda
+ {
+ myLed = 1;
+ }; break;
+ case 'B': // spegni LED su scheda
+ {
+ myLed = 0;
+ }; break;
+ case 'C': // accendi illuminazione a LED
+ {
+ Light = 1;
+ }; break;
+ case 'D': // spegni illuminazione a LED
+ {
+ Light = 0;
+ }; break;
+ case 'E': // Reset odometria e illuminazione
+ {
+ nCountRiseEdge = 0;
+ nOldCountRiseEdge = 0;
+ Light = 0;
+ fDistanzaPercorsa = 0.0;
+ fSpeed = 0.0;
+ myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,'0','0','0',',','0','0',0x04,'0','0','0', ',','0','0',0x05,0x30,0x30,0x30,0x30,0x30, 0x03);
+ }; break;
+ case 'F': // Toglie Reset da odometria e illuminazione
+ {
+ }; break;
+
+ default: break;
+ }
+ pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
+ cOldCommandBLE = cCommandBLE;
+ }
+ //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++
+ //Invert X
+ //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
+ //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
+ //Calcola R: R = (V+W) /2
+ //Calcola L: L= (V-W)/2
+ //Scala i valori di L e R in base all'hardware.
+ //invia i valori al robot.
+ // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote
+ if( (fX != fOldX) || (fY != fOldY))
+ {
+ fOldX = fX;
+ fOldY = fY;
+ // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
+ fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
+ fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
+ fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
+ fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
+
+ // diagnostica
+ pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
+ pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
+ pc.printf("> Velocita' Right R = %.2f\r\n", fR);
+ pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL);
+
+ // algoritmo di movimentazione delle ruote.
+ if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
+ {
+ fR =-fR;
+ // Vai indietro
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ }
+ else
+ {
+ if(fR >0)
+ {
+ // Vai avanti
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ }
+ else
+ {
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ }
+ }
+ PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
+ if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
+ {
+ fL =-fL;
+ // Vai indietro
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ }
+ else
+ {
+ if(fL >0)
+ {
+ // Vai avanti
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+
+ }
+ else
+ {
+ // spegni
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ }
+ }
+ AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
+ }
+ //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
+ } //while (true) Ciclo principale
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ FINE Ciclo Principale +++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++
+
+
+
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+ //++++++++++++++ INIZIO Test di calcolo e trasmissione distanza percorsa +++++++++++++++++++++++++++++++
+ /*
+ pc.printf("myButton = 1; iniziale\r\n");
+ while(myButton == 1);
+ pc.printf("myButton = 0; \r\n");
+ while(myButton == 0);
+ pc.printf("myButton = 1; finale\r\n\r\n");
+ fDistanzaPercorsa = 980.20;
+ nDistanzaPercorsa = fDistanzaPercorsa*100; // considera fino ai centesimi della distanza percorsa
+
+ // inizializza caDistanzaPercorsa[]
+ for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
+ {
+ caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
+ }
+
+ // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
+ nIndexDistanzaPercorsa = 0;
+ do
+ {
+ // generate digits in reverse order
+ caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0'; // get next digit
+
+ //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica
+ nIndexDistanzaPercorsa++;
+ }
+ while ((nDistanzaPercorsa /= 10) > 0) ;
+
+ // invia la distana percorsa al PC
+ pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c, \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
+
+ // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,..........................................................................................................,0x04,Data2,..............,0x05,Data3,...................,ETX;
+ //myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','0','0', ',','0',0x00,0x00,0x00,0x00,0x00,0x00, 0x03);
+ myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
+ while(true);
+ */
+ //+++++++++++++++ Fine Test di calcolo e trasmissione distanza percorsa ++++++++++++++++++++++++
+
+
+
+
+ //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++
+ /*
+ while(true)
+ {
+ if(cCommandBLE != cOldCommandBLE)
+ {
+
+ switch (cCommandBLE)
+ {
+ case 'A':
+ {
+ myLed = 1;
+ }; break;
+ case 'B':
+ {
+ myLed = 0;
+ }; break;
+ case 'C':
+ {
+ Light = 1;
+ }; break;
+ case 'D':
+ {
+ Light = 0;
+ }; break;
+ default: break;
+ }
+ // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
+ cOldCommandBLE = cCommandBLE;
+ }
+
+ } // while(true) Test comandi da BLE
+ */
+ //++++++++++++ FINE ricezione comandi BLE ++++++++++++
+
+ //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
+ /*
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+
+ fDutyCycle = 0.0;
+
+ // inizializza il pin PWM
+ //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
+ //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
+ //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
+ //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
+ while(true)
+ {
+ // Vai avanti Anteriore
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Avanti Anteriore\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai Indietro Anteriore
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+ PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Indietro Anteriore \r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai avanti Posteriore
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
+ pc.printf("Avanti Posteriore\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai Indietro Posteriore
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
+ pc.printf("Indietro Posteriore \r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai avanti Anteriore + Posteriore
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Avanti Anteriore + Posteriore\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai Indietro Anteriore + Posteriore
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Indietro Anteriore + Posteriore \r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai avanti Anteriore + Posteriore velocità ridotta
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
+ AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
+ pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Vai Indietro Anteriore + Posteriore velocità ridotta
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+ PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
+ AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
+ pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Ruota a destra
+ PostOutBI1 = 0;
+ PostOutBI2 = 1;
+ AntOutBI1 = 1;
+ AntOutBI2 = 0;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Ruota a Destra\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ // Ruota a sinistra
+ PostOutBI1 = 1;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 1;
+ PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+ AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+ pc.printf("Ruota a Sinistra\r\n");
+ wait (2);
+
+ // spegni
+ PostOutBI1 = 0;
+ PostOutBI2 = 0;
+ AntOutBI1 = 0;
+ AntOutBI2 = 0;
+ pc.printf("Fermo\r\n\r\n");
+ wait (3);
+
+ } // while(true) Test motore con PWM
+ */
+ //++++++++++++ FINE Test Motore con PWM ++++++++++++
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++ FINE Ciclo test +++++++++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++
+
+} // main()