Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev07_FUNZIONA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Mic-Rev03.cpp@7:7043da244e4b, 2019-08-08 (annotated)

Committer:
pinofal
Date:
Thu Aug 08 09:04:07 2019 +0000
Revision:
7:7043da244e4b
Da provare

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pinofal 7:7043da244e4b 1 // pilotaggio carrello tramite BLE.
pinofal 7:7043da244e4b 2 // testato su L476RG e F401RE
pinofal 7:7043da244e4b 3
pinofal 7:7043da244e4b 4 #include "mbed.h"
pinofal 7:7043da244e4b 5 #include<stdlib.h>
pinofal 7:7043da244e4b 6
pinofal 7:7043da244e4b 7
pinofal 7:7043da244e4b 8 // pi greco
pinofal 7:7043da244e4b 9 #define PI 3.14159265358979323846
pinofal 7:7043da244e4b 10
pinofal 7:7043da244e4b 11 // diametro della ruota in [metri]
pinofal 7:7043da244e4b 12 #define DIAMETRORUOTA (0.1)
pinofal 7:7043da244e4b 13
pinofal 7:7043da244e4b 14 // numero di impulsi per giro generati dall'encoder
pinofal 7:7043da244e4b 15 #define IMPULSIPERGIRO 4
pinofal 7:7043da244e4b 16
pinofal 7:7043da244e4b 17 // numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m]
pinofal 7:7043da244e4b 18 #define NUMCIFREDISTANZAPERCORSA 5
pinofal 7:7043da244e4b 19
pinofal 7:7043da244e4b 20 // 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]
pinofal 7:7043da244e4b 21 #define NUMCIFRESPEED 5
pinofal 7:7043da244e4b 22
pinofal 7:7043da244e4b 23 // intervallo di tempo in [msec], cui vengono contati gli impulsi di encoder per il calcolo della velocità
pinofal 7:7043da244e4b 24 #define DELTAT 200
pinofal 7:7043da244e4b 25
pinofal 7:7043da244e4b 26 // ticker per il calcolo della velocità
pinofal 7:7043da244e4b 27 //Ticker SpeedTicker;
pinofal 7:7043da244e4b 28
pinofal 7:7043da244e4b 29 // Timer per il calcolo della velocità. Nel periodo di tempo del timer, conta gli impulsi ricevuti come interrupt dall'encoder
pinofal 7:7043da244e4b 30 Timer TimerSpeed;
pinofal 7:7043da244e4b 31
pinofal 7:7043da244e4b 32 // tempo inizio intermedio e fine del timer che misura il tempo per il calcolo della velocità
pinofal 7:7043da244e4b 33 int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;
pinofal 7:7043da244e4b 34
pinofal 7:7043da244e4b 35 // Ticker per la simulazione di segnale proveniente da encoder sul motore
pinofal 7:7043da244e4b 36 Ticker SpeedTicker;
pinofal 7:7043da244e4b 37
pinofal 7:7043da244e4b 38 // Definizione periferica USB seriale del PC
pinofal 7:7043da244e4b 39 Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
pinofal 7:7043da244e4b 40
pinofal 7:7043da244e4b 41 // Definizione periferica seriale del Modulo BLE ELETT114A
pinofal 7:7043da244e4b 42 Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps
pinofal 7:7043da244e4b 43
pinofal 7:7043da244e4b 44 // Input di Reset per il Modulo BLE HC-05
pinofal 7:7043da244e4b 45 DigitalOut BleRst(PA_8);
pinofal 7:7043da244e4b 46
pinofal 7:7043da244e4b 47 // User Button, LED
pinofal 7:7043da244e4b 48 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
pinofal 7:7043da244e4b 49 DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
pinofal 7:7043da244e4b 50
pinofal 7:7043da244e4b 51 // output digitale per pilotaggio illuminazione a LED
pinofal 7:7043da244e4b 52 DigitalOut Light(PA_0);
pinofal 7:7043da244e4b 53 //DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20
pinofal 7:7043da244e4b 54 InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore.
pinofal 7:7043da244e4b 55
pinofal 7:7043da244e4b 56 // carattere letto dalla seriale del PC
pinofal 7:7043da244e4b 57 char cReadChar;
pinofal 7:7043da244e4b 58
pinofal 7:7043da244e4b 59 // DutyCycle del segnale PWM
pinofal 7:7043da244e4b 60 float fDutyCycle;
pinofal 7:7043da244e4b 61
pinofal 7:7043da244e4b 62 // variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot
pinofal 7:7043da244e4b 63 volatile int nCountRiseEdge;
pinofal 7:7043da244e4b 64 volatile int nOldCountRiseEdge;
pinofal 7:7043da244e4b 65
pinofal 7:7043da244e4b 66 // Input/Output
pinofal 7:7043da244e4b 67 DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
pinofal 7:7043da244e4b 68 PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore
pinofal 7:7043da244e4b 69 //DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
pinofal 7:7043da244e4b 70 DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 7:7043da244e4b 71 DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
pinofal 7:7043da244e4b 72
pinofal 7:7043da244e4b 73 DigitalOut AntOutBI1 (PB_4); // Output 1 per pilotaggio input BI1 del Motore B Anteriore
pinofal 7:7043da244e4b 74 PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore
pinofal 7:7043da244e4b 75 //DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
pinofal 7:7043da244e4b 76 DigitalOut AntOutBI2 (PB_3); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 7:7043da244e4b 77 DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
pinofal 7:7043da244e4b 78
pinofal 7:7043da244e4b 79
pinofal 7:7043da244e4b 80 //carattere di comando ricevuto dal BLE
pinofal 7:7043da244e4b 81 volatile char cCommandBLE; // cambia nella routine di interrupt
pinofal 7:7043da244e4b 82 // memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
pinofal 7:7043da244e4b 83 char cOldCommandBLE;
pinofal 7:7043da244e4b 84
pinofal 7:7043da244e4b 85 // coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
pinofal 7:7043da244e4b 86 volatile double fTeta;
pinofal 7:7043da244e4b 87 volatile double fRo;
pinofal 7:7043da244e4b 88
pinofal 7:7043da244e4b 89 // coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
pinofal 7:7043da244e4b 90 volatile double fX, fY;
pinofal 7:7043da244e4b 91 // memorizza ultimi valori delle coordinate del joistick
pinofal 7:7043da244e4b 92 double fOldX, fOldY;
pinofal 7:7043da244e4b 93
pinofal 7:7043da244e4b 94 // variabili ausiliarie per l'algoritmo di posizionamento
pinofal 7:7043da244e4b 95 double fV, fW;
pinofal 7:7043da244e4b 96
pinofal 7:7043da244e4b 97 // velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
pinofal 7:7043da244e4b 98 double fR, fL;
pinofal 7:7043da244e4b 99
pinofal 7:7043da244e4b 100 // distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore
pinofal 7:7043da244e4b 101 double fDistanzaPercorsa;
pinofal 7:7043da244e4b 102
pinofal 7:7043da244e4b 103 // velocità calcolata gli impulsi contati in un intervallo DELTAT msec
pinofal 7:7043da244e4b 104 double fSpeed;
pinofal 7:7043da244e4b 105
pinofal 7:7043da244e4b 106 // Scopi diagnostici: Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
pinofal 7:7043da244e4b 107 // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
pinofal 7:7043da244e4b 108 double fDeltaTick;
pinofal 7:7043da244e4b 109
pinofal 7:7043da244e4b 110 // 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]
pinofal 7:7043da244e4b 111 //char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
pinofal 7:7043da244e4b 112 //int nIndexDistanzaPercorsa;
pinofal 7:7043da244e4b 113
pinofal 7:7043da244e4b 114 //**********************************************/
pinofal 7:7043da244e4b 115 // IRQ associata a Rx da BLE
pinofal 7:7043da244e4b 116 //**********************************************/
pinofal 7:7043da244e4b 117 void BLERxInterrupt(void)
pinofal 7:7043da244e4b 118 {
pinofal 7:7043da244e4b 119 // array per la ricezione dei messaggi da seriale
pinofal 7:7043da244e4b 120 char cReadChar;
pinofal 7:7043da244e4b 121
pinofal 7:7043da244e4b 122 //indice per i cicli
pinofal 7:7043da244e4b 123 int nIndex;
pinofal 7:7043da244e4b 124
pinofal 7:7043da244e4b 125 // array per la ricezione dei messaggi da seriale
pinofal 7:7043da244e4b 126 char caRxPacket[8];
pinofal 7:7043da244e4b 127 //int nRxPacketSize;
pinofal 7:7043da244e4b 128
pinofal 7:7043da244e4b 129 // variabile per estrarre le cifre della distanza percorsa. La distanza Percorsa viene calcolata nel MAIN con la varibile fDistanzaPercorsa
pinofal 7:7043da244e4b 130 int nDistanzaPercorsa;
pinofal 7:7043da244e4b 131 // 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]
pinofal 7:7043da244e4b 132 char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
pinofal 7:7043da244e4b 133 int nIndexDistanzaPercorsa;
pinofal 7:7043da244e4b 134
pinofal 7:7043da244e4b 135 // variabile per estrarre le cifre della velocità di percorrenza. La velocità viene calcolata nel MAIN con la varibile fSpeed
pinofal 7:7043da244e4b 136 int nSpeed;
pinofal 7:7043da244e4b 137 // 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]
pinofal 7:7043da244e4b 138 char caSpeed[NUMCIFRESPEED];
pinofal 7:7043da244e4b 139 int nIndexSpeed;
pinofal 7:7043da244e4b 140
pinofal 7:7043da244e4b 141
pinofal 7:7043da244e4b 142 //pc.printf("BLE RxInterrupt: \n\r");
pinofal 7:7043da244e4b 143
pinofal 7:7043da244e4b 144 // ricevi caratteri su seriale, se disponibili
pinofal 7:7043da244e4b 145 while((myBLE.readable()))
pinofal 7:7043da244e4b 146 {
pinofal 7:7043da244e4b 147 // acquisice stringa in input e relativa dimensione
pinofal 7:7043da244e4b 148 cReadChar = myBLE.getc(); // Read character
pinofal 7:7043da244e4b 149 if(cReadChar == 0x02)
pinofal 7:7043da244e4b 150 {
pinofal 7:7043da244e4b 151
pinofal 7:7043da244e4b 152 //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 153 //pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
pinofal 7:7043da244e4b 154
pinofal 7:7043da244e4b 155 //-- command will be 8 bytes for joystick values
pinofal 7:7043da244e4b 156 //-- command will be 3 bytes for button change event
pinofal 7:7043da244e4b 157 //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
pinofal 7:7043da244e4b 158
pinofal 7:7043da244e4b 159
pinofal 7:7043da244e4b 160 caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
pinofal 7:7043da244e4b 161 cReadChar = myBLE.getc(); // legge il secondo carattere
pinofal 7:7043da244e4b 162 if(cReadChar > 0x40)
pinofal 7:7043da244e4b 163 {
pinofal 7:7043da244e4b 164 // Button:
pinofal 7:7043da244e4b 165 //-- Button events send a single character in a 3-byte packet
pinofal 7:7043da244e4b 166 //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
pinofal 7:7043da244e4b 167 //-- B2 uses "C" for changed to on, "D" for changed to off
pinofal 7:7043da244e4b 168 //-- B3..B6 follow in order; valid button even characters are "A".."L"
pinofal 7:7043da244e4b 169 caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
pinofal 7:7043da244e4b 170 caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX
pinofal 7:7043da244e4b 171 // passa il comando ricevuto nella variabile globale
pinofal 7:7043da244e4b 172 cCommandBLE = caRxPacket[1];
pinofal 7:7043da244e4b 173
pinofal 7:7043da244e4b 174 // Diagnostica
pinofal 7:7043da244e4b 175 /*
pinofal 7:7043da244e4b 176 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
pinofal 7:7043da244e4b 177 */
pinofal 7:7043da244e4b 178 //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 179 }
pinofal 7:7043da244e4b 180 else
pinofal 7:7043da244e4b 181 {
pinofal 7:7043da244e4b 182 //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggi Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 183 // Joystick:
pinofal 7:7043da244e4b 184 //-- 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
pinofal 7:7043da244e4b 185 //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
pinofal 7:7043da244e4b 186 //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
pinofal 7:7043da244e4b 187 caRxPacket[1] = cReadChar; // memorizza il secondo carattere
pinofal 7:7043da244e4b 188 for(nIndex=2; nIndex<8; nIndex++)
pinofal 7:7043da244e4b 189 {
pinofal 7:7043da244e4b 190 caRxPacket[nIndex] = myBLE.getc();
pinofal 7:7043da244e4b 191 }
pinofal 7:7043da244e4b 192 // dal messaggio estrae e visualizza le coordinate cartesiane
pinofal 7:7043da244e4b 193 fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
pinofal 7:7043da244e4b 194 fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
pinofal 7:7043da244e4b 195 // converte la posizione del joistick in coordinate polari
pinofal 7:7043da244e4b 196 fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
pinofal 7:7043da244e4b 197 if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
pinofal 7:7043da244e4b 198 fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
pinofal 7:7043da244e4b 199
pinofal 7:7043da244e4b 200 // diagnostica
pinofal 7:7043da244e4b 201 /*
pinofal 7:7043da244e4b 202 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
pinofal 7:7043da244e4b 203 pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
pinofal 7:7043da244e4b 204 pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
pinofal 7:7043da244e4b 205 */
pinofal 7:7043da244e4b 206 }
pinofal 7:7043da244e4b 207 //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 208
pinofal 7:7043da244e4b 209 //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 210 //fDistanzaPercorsa = 980.20; // diagnostica
pinofal 7:7043da244e4b 211 nDistanzaPercorsa = fDistanzaPercorsa*100; // moltiplica per 100 per considerare fino ai centesimi della distanza percorsa
pinofal 7:7043da244e4b 212 nSpeed = fSpeed*100; // moltiplica per 100 per considerare fino ai centesimi della velocità
pinofal 7:7043da244e4b 213
pinofal 7:7043da244e4b 214 // inizializza caDistanzaPercorsa[]
pinofal 7:7043da244e4b 215 for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
pinofal 7:7043da244e4b 216 {
pinofal 7:7043da244e4b 217 caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
pinofal 7:7043da244e4b 218 }
pinofal 7:7043da244e4b 219
pinofal 7:7043da244e4b 220 // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
pinofal 7:7043da244e4b 221 nIndexDistanzaPercorsa = 0;
pinofal 7:7043da244e4b 222 do
pinofal 7:7043da244e4b 223 {
pinofal 7:7043da244e4b 224 // genera digits della distanza percorsa
pinofal 7:7043da244e4b 225 caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0';
pinofal 7:7043da244e4b 226
pinofal 7:7043da244e4b 227 //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica
pinofal 7:7043da244e4b 228 nIndexDistanzaPercorsa++;
pinofal 7:7043da244e4b 229 }
pinofal 7:7043da244e4b 230 while ((nDistanzaPercorsa /= 10) > 0) ;
pinofal 7:7043da244e4b 231 // Diagnostica: visualizza la distanza percorsa sul PC
pinofal 7:7043da244e4b 232 //pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
pinofal 7:7043da244e4b 233 //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 234
pinofal 7:7043da244e4b 235 //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 236 // inizializza caSpeed[]
pinofal 7:7043da244e4b 237 for(nIndexSpeed = 0; nIndexSpeed < NUMCIFREDISTANZAPERCORSA; nIndexSpeed++)
pinofal 7:7043da244e4b 238 {
pinofal 7:7043da244e4b 239 caSpeed[nIndexSpeed] = '0';
pinofal 7:7043da244e4b 240 }
pinofal 7:7043da244e4b 241
pinofal 7:7043da244e4b 242 // estrae le singole cifre di fSpeed, fino alla seconda cifra decimale
pinofal 7:7043da244e4b 243 nIndexSpeed = 0;
pinofal 7:7043da244e4b 244 do
pinofal 7:7043da244e4b 245 {
pinofal 7:7043da244e4b 246 // genera digits della velocità
pinofal 7:7043da244e4b 247 caSpeed[nIndexSpeed] = nSpeed % 10 + '0';
pinofal 7:7043da244e4b 248
pinofal 7:7043da244e4b 249 //pc.printf("caSpeed[%d]: %c \n\r", nIndexSpeed, caSpeed[nIndexSpeed]); // diagnostica
pinofal 7:7043da244e4b 250 nIndexSpeed++;
pinofal 7:7043da244e4b 251 }
pinofal 7:7043da244e4b 252 while ((nSpeed /= 10) > 0) ;
pinofal 7:7043da244e4b 253 //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 254
pinofal 7:7043da244e4b 255 //++++++++++++++++++++++++++++++++++++++++++++ INIZIO Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 256
pinofal 7:7043da244e4b 257 // Diagnostica: visualizza la velocità sul PC
pinofal 7:7043da244e4b 258 //pc.printf("Speed: %c,%c,%c,%c,%c \n\r", caSpeed[4], caSpeed[3], caSpeed[2], caSpeed[1], caSpeed[0]); // diagnostica
pinofal 7:7043da244e4b 259
pinofal 7:7043da244e4b 260 // Struttura Nominale del comando da inviare al robot STX ,B6, B5, B4, B3 B2 B1 0x01,Data1,...........................................................................................................,0x04,Data2,.....................................................,0x05,Data3,...................,ETX;
pinofal 7:7043da244e4b 261 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);
pinofal 7:7043da244e4b 262
pinofal 7:7043da244e4b 263 //++++++++++++++++++++++++++++++++++++++++++++ FINE Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 264 }
pinofal 7:7043da244e4b 265 }
pinofal 7:7043da244e4b 266 }
pinofal 7:7043da244e4b 267
pinofal 7:7043da244e4b 268 /**********************************************/
pinofal 7:7043da244e4b 269 // IRQ associata a Rx da PC
pinofal 7:7043da244e4b 270 //**********************************************/
pinofal 7:7043da244e4b 271 void pcRxInterrupt(void)
pinofal 7:7043da244e4b 272 {
pinofal 7:7043da244e4b 273 // array per la ricezione dei messaggi da seriale
pinofal 7:7043da244e4b 274 char cReadChar;
pinofal 7:7043da244e4b 275
pinofal 7:7043da244e4b 276 // ricevi caratteri su seriale, se disponibili
pinofal 7:7043da244e4b 277 while((pc.readable()))
pinofal 7:7043da244e4b 278 {
pinofal 7:7043da244e4b 279 // acquisice stringa in input e relativa dimensione
pinofal 7:7043da244e4b 280 cReadChar = pc.getc(); // read character from PC
pinofal 7:7043da244e4b 281 //myBLE.putc(cReadChar); // Diagnostica: write char to BLE
pinofal 7:7043da244e4b 282 //pc.putc(cReadChar); // Diagnostica: write char to PC
pinofal 7:7043da244e4b 283
pinofal 7:7043da244e4b 284 //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica
pinofal 7:7043da244e4b 285 if(cReadChar == '0') // se scrivo '0', invia questa stringa
pinofal 7:7043da244e4b 286 {
pinofal 7:7043da244e4b 287 // DIAGNOSTICA:
pinofal 7:7043da244e4b 288 // pc.printf("W>: Inviato comando a BLE\n\r"); // diagnostica
pinofal 7:7043da244e4b 289 //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
pinofal 7:7043da244e4b 290 //-- button status is binary formatted string (no indicator)
pinofal 7:7043da244e4b 291 //-- data fields sent as strings
pinofal 7:7043da244e4b 292 //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
pinofal 7:7043da244e4b 293 // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
pinofal 7:7043da244e4b 294 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);
pinofal 7:7043da244e4b 295 }
pinofal 7:7043da244e4b 296 }
pinofal 7:7043da244e4b 297 }
pinofal 7:7043da244e4b 298
pinofal 7:7043da244e4b 299
pinofal 7:7043da244e4b 300 /**************************************************************************************/
pinofal 7:7043da244e4b 301 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
pinofal 7:7043da244e4b 302 /**************************************************************************************/
pinofal 7:7043da244e4b 303 void riseEncoderIRQ()
pinofal 7:7043da244e4b 304 {
pinofal 7:7043da244e4b 305 nCountRiseEdge++;
pinofal 7:7043da244e4b 306 }
pinofal 7:7043da244e4b 307
pinofal 7:7043da244e4b 308 /****************************************************************************************/
pinofal 7:7043da244e4b 309 /* Diagnostica: */
pinofal 7:7043da244e4b 310 /* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO */
pinofal 7:7043da244e4b 311 /* Routine di gestione del ticker per simulare encoder */
pinofal 7:7043da244e4b 312 /* Simula il segnale di encoder ricevuto con un determinato DELTAT */
pinofal 7:7043da244e4b 313 /* A robot fermo, il segnale di encoder non genera interrupt. */
pinofal 7:7043da244e4b 314 /* Questo Ticker simula l'arrivo del segnale da encoder */
pinofal 7:7043da244e4b 315 /****************************************************************************************/
pinofal 7:7043da244e4b 316 void SpeedCalculate()
pinofal 7:7043da244e4b 317 {
pinofal 7:7043da244e4b 318 // ad ogni tick viene ricevuto un impulso da encoder.
pinofal 7:7043da244e4b 319 // velocità = (Spazio per ogni tick)/(tempo per ogni tick)
pinofal 7:7043da244e4b 320 // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
pinofal 7:7043da244e4b 321
pinofal 7:7043da244e4b 322 // simula impulso inviato dall'encoder
pinofal 7:7043da244e4b 323 nCountRiseEdge++;
pinofal 7:7043da244e4b 324 }
pinofal 7:7043da244e4b 325
pinofal 7:7043da244e4b 326
pinofal 7:7043da244e4b 327 /**********/
pinofal 7:7043da244e4b 328 /* MAIN */
pinofal 7:7043da244e4b 329 /**********/
pinofal 7:7043da244e4b 330 int main()
pinofal 7:7043da244e4b 331 {
pinofal 7:7043da244e4b 332
pinofal 7:7043da244e4b 333 // messaggio di benvenuto
pinofal 7:7043da244e4b 334 pc.printf("\r\n************ Hallo ****************** \r\n");
pinofal 7:7043da244e4b 335 pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
pinofal 7:7043da244e4b 336
pinofal 7:7043da244e4b 337 // inizializza variabili da BLE
pinofal 7:7043da244e4b 338 cCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 7:7043da244e4b 339 cOldCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 7:7043da244e4b 340 fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 7:7043da244e4b 341 fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 7:7043da244e4b 342 fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 7:7043da244e4b 343 fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 7:7043da244e4b 344
pinofal 7:7043da244e4b 345 // inizializza il BLE
pinofal 7:7043da244e4b 346 BleRst = 0;
pinofal 7:7043da244e4b 347 wait_ms(100);
pinofal 7:7043da244e4b 348 BleRst = 1;
pinofal 7:7043da244e4b 349 cCommandBLE = '0';
pinofal 7:7043da244e4b 350 cOldCommandBLE = '0';
pinofal 7:7043da244e4b 351
pinofal 7:7043da244e4b 352 // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore
pinofal 7:7043da244e4b 353 PostOutPWB.period_us(10); // periodo del PWM Posteriore
pinofal 7:7043da244e4b 354 PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
pinofal 7:7043da244e4b 355 AntOutPWB.period_us(10); // periodo del PWM Anteriore
pinofal 7:7043da244e4b 356 AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 7:7043da244e4b 357
pinofal 7:7043da244e4b 358 // inizializza variabili
pinofal 7:7043da244e4b 359 fDistanzaPercorsa = 0.0;
pinofal 7:7043da244e4b 360 fSpeed = 0.0;
pinofal 7:7043da244e4b 361
pinofal 7:7043da244e4b 362 // Attiva la IRQ per la RX su seriale
pinofal 7:7043da244e4b 363 myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
pinofal 7:7043da244e4b 364 pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
pinofal 7:7043da244e4b 365
pinofal 7:7043da244e4b 366 // attiva un ticker per simulare robot in movimento.
pinofal 7:7043da244e4b 367 //+++++++++ INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
pinofal 7:7043da244e4b 368 /*
pinofal 7:7043da244e4b 369 fDeltaTick = 0.05; // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
pinofal 7:7043da244e4b 370 SpeedTicker.attach(&SpeedCalculate,fDeltaTick); // Diagnostica
pinofal 7:7043da244e4b 371 */
pinofal 7:7043da244e4b 372 //+++++++++ FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
pinofal 7:7043da244e4b 373
pinofal 7:7043da244e4b 374 //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 375 // definisci il mode del segnale digitale di EncoderA
pinofal 7:7043da244e4b 376 InEncoderA.mode(PullUp);
pinofal 7:7043da244e4b 377
pinofal 7:7043da244e4b 378 // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
pinofal 7:7043da244e4b 379 InEncoderA.rise(&riseEncoderIRQ);
pinofal 7:7043da244e4b 380
pinofal 7:7043da244e4b 381 // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
pinofal 7:7043da244e4b 382 nCountRiseEdge=0;
pinofal 7:7043da244e4b 383 nOldCountRiseEdge=0;
pinofal 7:7043da244e4b 384 //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 385
pinofal 7:7043da244e4b 386
pinofal 7:7043da244e4b 387 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 388 //++++++++++++++ INIZIO Ciclo Principale +++++++++++++
pinofal 7:7043da244e4b 389 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 390
pinofal 7:7043da244e4b 391 while(true)
pinofal 7:7043da244e4b 392 {
pinofal 7:7043da244e4b 393 //+++++++++++++++++++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 394
pinofal 7:7043da244e4b 395 // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi la velocità di spostamento del robot
pinofal 7:7043da244e4b 396 InEncoderA.enable_irq();
pinofal 7:7043da244e4b 397
pinofal 7:7043da244e4b 398 // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
pinofal 7:7043da244e4b 399 TimerSpeed.start();
pinofal 7:7043da244e4b 400 nTimerStart=TimerSpeed.read_ms();
pinofal 7:7043da244e4b 401
pinofal 7:7043da244e4b 402 // per 100ms conta gliimpulsi sull'encoder
pinofal 7:7043da244e4b 403 while( (nTimerCurrent-nTimerStart) < DELTAT) // attende il passare di DELTAT millisec
pinofal 7:7043da244e4b 404 {
pinofal 7:7043da244e4b 405 nTimerCurrent=TimerSpeed.read_ms();
pinofal 7:7043da244e4b 406 // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));
pinofal 7:7043da244e4b 407 }
pinofal 7:7043da244e4b 408 TimerSpeed.stop(); // ferma il timer
pinofal 7:7043da244e4b 409
pinofal 7:7043da244e4b 410 // disabilita interrupt sul segnale di encoder per contare il numero di impulsi. Disattivandolo, si evita di continuare a mandare in interrupt il processore
pinofal 7:7043da244e4b 411 InEncoderA.enable_irq();
pinofal 7:7043da244e4b 412 //+++++++++++++++++++++++++++++ FINE calcola spostamento con encoder sul motore ++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 413
pinofal 7:7043da244e4b 414 //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 415 //nCountRiseEdge++; //----diagnostica
pinofal 7:7043da244e4b 416 // se nella IRQ, durante il periodo di calcolo della velocità, sono stati contati fronti di salita dell'encoder, il robot si sta muovendo
pinofal 7:7043da244e4b 417 if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo
pinofal 7:7043da244e4b 418 {
pinofal 7:7043da244e4b 419 // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
pinofal 7:7043da244e4b 420 fDistanzaPercorsa = (PI*DIAMETRORUOTA/IMPULSIPERGIRO)*nCountRiseEdge;
pinofal 7:7043da244e4b 421
pinofal 7:7043da244e4b 422 // calcola la velocità in [m/sec]. DELTAT è in [msec] lo spostamento è in [m]
pinofal 7:7043da244e4b 423 fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/float(DELTAT/1000.0);
pinofal 7:7043da244e4b 424
pinofal 7:7043da244e4b 425 // ricorda lo spostamento
pinofal 7:7043da244e4b 426 nOldCountRiseEdge = nCountRiseEdge;
pinofal 7:7043da244e4b 427 }
pinofal 7:7043da244e4b 428 //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 429
pinofal 7:7043da244e4b 430 //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 431 if(cCommandBLE != cOldCommandBLE)
pinofal 7:7043da244e4b 432 {
pinofal 7:7043da244e4b 433 switch (cCommandBLE)
pinofal 7:7043da244e4b 434 {
pinofal 7:7043da244e4b 435 case 'A': // accendi LED su scheda
pinofal 7:7043da244e4b 436 {
pinofal 7:7043da244e4b 437 myLed = 1;
pinofal 7:7043da244e4b 438 }; break;
pinofal 7:7043da244e4b 439 case 'B': // spegni LED su scheda
pinofal 7:7043da244e4b 440 {
pinofal 7:7043da244e4b 441 myLed = 0;
pinofal 7:7043da244e4b 442 }; break;
pinofal 7:7043da244e4b 443 case 'C': // accendi illuminazione a LED
pinofal 7:7043da244e4b 444 {
pinofal 7:7043da244e4b 445 Light = 1;
pinofal 7:7043da244e4b 446 }; break;
pinofal 7:7043da244e4b 447 case 'D': // spegni illuminazione a LED
pinofal 7:7043da244e4b 448 {
pinofal 7:7043da244e4b 449 Light = 0;
pinofal 7:7043da244e4b 450 }; break;
pinofal 7:7043da244e4b 451 case 'E': // Reset odometria e illuminazione
pinofal 7:7043da244e4b 452 {
pinofal 7:7043da244e4b 453 nCountRiseEdge = 0;
pinofal 7:7043da244e4b 454 nOldCountRiseEdge = 0;
pinofal 7:7043da244e4b 455 Light = 0;
pinofal 7:7043da244e4b 456 fDistanzaPercorsa = 0.0;
pinofal 7:7043da244e4b 457 fSpeed = 0.0;
pinofal 7:7043da244e4b 458 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);
pinofal 7:7043da244e4b 459 }; break;
pinofal 7:7043da244e4b 460 case 'F': // Toglie Reset da odometria e illuminazione
pinofal 7:7043da244e4b 461 {
pinofal 7:7043da244e4b 462 }; break;
pinofal 7:7043da244e4b 463
pinofal 7:7043da244e4b 464 default: break;
pinofal 7:7043da244e4b 465 }
pinofal 7:7043da244e4b 466 pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
pinofal 7:7043da244e4b 467 cOldCommandBLE = cCommandBLE;
pinofal 7:7043da244e4b 468 }
pinofal 7:7043da244e4b 469 //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 470
pinofal 7:7043da244e4b 471 //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++
pinofal 7:7043da244e4b 472 //Invert X
pinofal 7:7043da244e4b 473 //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
pinofal 7:7043da244e4b 474 //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
pinofal 7:7043da244e4b 475 //Calcola R: R = (V+W) /2
pinofal 7:7043da244e4b 476 //Calcola L: L= (V-W)/2
pinofal 7:7043da244e4b 477 //Scala i valori di L e R in base all'hardware.
pinofal 7:7043da244e4b 478 //invia i valori al robot.
pinofal 7:7043da244e4b 479 // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote
pinofal 7:7043da244e4b 480 if( (fX != fOldX) || (fY != fOldY))
pinofal 7:7043da244e4b 481 {
pinofal 7:7043da244e4b 482 fOldX = fX;
pinofal 7:7043da244e4b 483 fOldY = fY;
pinofal 7:7043da244e4b 484 // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
pinofal 7:7043da244e4b 485 fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
pinofal 7:7043da244e4b 486 fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
pinofal 7:7043da244e4b 487 fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
pinofal 7:7043da244e4b 488 fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
pinofal 7:7043da244e4b 489
pinofal 7:7043da244e4b 490 // diagnostica
pinofal 7:7043da244e4b 491 pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
pinofal 7:7043da244e4b 492 pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
pinofal 7:7043da244e4b 493 pc.printf("> Velocita' Right R = %.2f\r\n", fR);
pinofal 7:7043da244e4b 494 pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL);
pinofal 7:7043da244e4b 495
pinofal 7:7043da244e4b 496 // algoritmo di movimentazione delle ruote.
pinofal 7:7043da244e4b 497 if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
pinofal 7:7043da244e4b 498 {
pinofal 7:7043da244e4b 499 fR =-fR;
pinofal 7:7043da244e4b 500 // Vai indietro
pinofal 7:7043da244e4b 501 PostOutBI1 = 1;
pinofal 7:7043da244e4b 502 PostOutBI2 = 0;
pinofal 7:7043da244e4b 503 }
pinofal 7:7043da244e4b 504 else
pinofal 7:7043da244e4b 505 {
pinofal 7:7043da244e4b 506 if(fR >0)
pinofal 7:7043da244e4b 507 {
pinofal 7:7043da244e4b 508 // Vai avanti
pinofal 7:7043da244e4b 509 PostOutBI1 = 0;
pinofal 7:7043da244e4b 510 PostOutBI2 = 1;
pinofal 7:7043da244e4b 511 }
pinofal 7:7043da244e4b 512 else
pinofal 7:7043da244e4b 513 {
pinofal 7:7043da244e4b 514 // spegni
pinofal 7:7043da244e4b 515 PostOutBI1 = 0;
pinofal 7:7043da244e4b 516 PostOutBI2 = 0;
pinofal 7:7043da244e4b 517 }
pinofal 7:7043da244e4b 518 }
pinofal 7:7043da244e4b 519 PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
pinofal 7:7043da244e4b 520 if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
pinofal 7:7043da244e4b 521 {
pinofal 7:7043da244e4b 522 fL =-fL;
pinofal 7:7043da244e4b 523 // Vai indietro
pinofal 7:7043da244e4b 524 AntOutBI1 = 1;
pinofal 7:7043da244e4b 525 AntOutBI2 = 0;
pinofal 7:7043da244e4b 526 }
pinofal 7:7043da244e4b 527 else
pinofal 7:7043da244e4b 528 {
pinofal 7:7043da244e4b 529 if(fL >0)
pinofal 7:7043da244e4b 530 {
pinofal 7:7043da244e4b 531 // Vai avanti
pinofal 7:7043da244e4b 532 AntOutBI1 = 0;
pinofal 7:7043da244e4b 533 AntOutBI2 = 1;
pinofal 7:7043da244e4b 534
pinofal 7:7043da244e4b 535 }
pinofal 7:7043da244e4b 536 else
pinofal 7:7043da244e4b 537 {
pinofal 7:7043da244e4b 538 // spegni
pinofal 7:7043da244e4b 539 AntOutBI1 = 0;
pinofal 7:7043da244e4b 540 AntOutBI2 = 0;
pinofal 7:7043da244e4b 541 }
pinofal 7:7043da244e4b 542 }
pinofal 7:7043da244e4b 543 AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 7:7043da244e4b 544 }
pinofal 7:7043da244e4b 545 //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 546 } //while (true) Ciclo principale
pinofal 7:7043da244e4b 547
pinofal 7:7043da244e4b 548 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 549 //++++++++++++++ FINE Ciclo Principale +++++++++++++
pinofal 7:7043da244e4b 550 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 551
pinofal 7:7043da244e4b 552
pinofal 7:7043da244e4b 553
pinofal 7:7043da244e4b 554
pinofal 7:7043da244e4b 555 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 556 //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++
pinofal 7:7043da244e4b 557 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 558
pinofal 7:7043da244e4b 559 //++++++++++++++ INIZIO Test di calcolo e trasmissione distanza percorsa +++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 560 /*
pinofal 7:7043da244e4b 561 pc.printf("myButton = 1; iniziale\r\n");
pinofal 7:7043da244e4b 562 while(myButton == 1);
pinofal 7:7043da244e4b 563 pc.printf("myButton = 0; \r\n");
pinofal 7:7043da244e4b 564 while(myButton == 0);
pinofal 7:7043da244e4b 565 pc.printf("myButton = 1; finale\r\n\r\n");
pinofal 7:7043da244e4b 566 fDistanzaPercorsa = 980.20;
pinofal 7:7043da244e4b 567 nDistanzaPercorsa = fDistanzaPercorsa*100; // considera fino ai centesimi della distanza percorsa
pinofal 7:7043da244e4b 568
pinofal 7:7043da244e4b 569 // inizializza caDistanzaPercorsa[]
pinofal 7:7043da244e4b 570 for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
pinofal 7:7043da244e4b 571 {
pinofal 7:7043da244e4b 572 caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
pinofal 7:7043da244e4b 573 }
pinofal 7:7043da244e4b 574
pinofal 7:7043da244e4b 575 // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
pinofal 7:7043da244e4b 576 nIndexDistanzaPercorsa = 0;
pinofal 7:7043da244e4b 577 do
pinofal 7:7043da244e4b 578 {
pinofal 7:7043da244e4b 579 // generate digits in reverse order
pinofal 7:7043da244e4b 580 caDistanzaPercorsa[nIndexDistanzaPercorsa] = nDistanzaPercorsa % 10 + '0'; // get next digit
pinofal 7:7043da244e4b 581
pinofal 7:7043da244e4b 582 //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]); // diagnostica
pinofal 7:7043da244e4b 583 nIndexDistanzaPercorsa++;
pinofal 7:7043da244e4b 584 }
pinofal 7:7043da244e4b 585 while ((nDistanzaPercorsa /= 10) > 0) ;
pinofal 7:7043da244e4b 586
pinofal 7:7043da244e4b 587 // invia la distana percorsa al PC
pinofal 7:7043da244e4b 588 pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c, \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
pinofal 7:7043da244e4b 589
pinofal 7:7043da244e4b 590 // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,..........................................................................................................,0x04,Data2,..............,0x05,Data3,...................,ETX;
pinofal 7:7043da244e4b 591 //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);
pinofal 7:7043da244e4b 592 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);
pinofal 7:7043da244e4b 593 while(true);
pinofal 7:7043da244e4b 594 */
pinofal 7:7043da244e4b 595 //+++++++++++++++ Fine Test di calcolo e trasmissione distanza percorsa ++++++++++++++++++++++++
pinofal 7:7043da244e4b 596
pinofal 7:7043da244e4b 597
pinofal 7:7043da244e4b 598
pinofal 7:7043da244e4b 599
pinofal 7:7043da244e4b 600 //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 601 /*
pinofal 7:7043da244e4b 602 while(true)
pinofal 7:7043da244e4b 603 {
pinofal 7:7043da244e4b 604 if(cCommandBLE != cOldCommandBLE)
pinofal 7:7043da244e4b 605 {
pinofal 7:7043da244e4b 606
pinofal 7:7043da244e4b 607 switch (cCommandBLE)
pinofal 7:7043da244e4b 608 {
pinofal 7:7043da244e4b 609 case 'A':
pinofal 7:7043da244e4b 610 {
pinofal 7:7043da244e4b 611 myLed = 1;
pinofal 7:7043da244e4b 612 }; break;
pinofal 7:7043da244e4b 613 case 'B':
pinofal 7:7043da244e4b 614 {
pinofal 7:7043da244e4b 615 myLed = 0;
pinofal 7:7043da244e4b 616 }; break;
pinofal 7:7043da244e4b 617 case 'C':
pinofal 7:7043da244e4b 618 {
pinofal 7:7043da244e4b 619 Light = 1;
pinofal 7:7043da244e4b 620 }; break;
pinofal 7:7043da244e4b 621 case 'D':
pinofal 7:7043da244e4b 622 {
pinofal 7:7043da244e4b 623 Light = 0;
pinofal 7:7043da244e4b 624 }; break;
pinofal 7:7043da244e4b 625 default: break;
pinofal 7:7043da244e4b 626 }
pinofal 7:7043da244e4b 627 // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
pinofal 7:7043da244e4b 628 cOldCommandBLE = cCommandBLE;
pinofal 7:7043da244e4b 629 }
pinofal 7:7043da244e4b 630
pinofal 7:7043da244e4b 631 } // while(true) Test comandi da BLE
pinofal 7:7043da244e4b 632 */
pinofal 7:7043da244e4b 633 //++++++++++++ FINE ricezione comandi BLE ++++++++++++
pinofal 7:7043da244e4b 634
pinofal 7:7043da244e4b 635 //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
pinofal 7:7043da244e4b 636 /*
pinofal 7:7043da244e4b 637 PostOutBI1 = 0;
pinofal 7:7043da244e4b 638 PostOutBI2 = 0;
pinofal 7:7043da244e4b 639 AntOutBI1 = 0;
pinofal 7:7043da244e4b 640 AntOutBI2 = 0;
pinofal 7:7043da244e4b 641
pinofal 7:7043da244e4b 642 fDutyCycle = 0.0;
pinofal 7:7043da244e4b 643
pinofal 7:7043da244e4b 644 // inizializza il pin PWM
pinofal 7:7043da244e4b 645 //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 7:7043da244e4b 646 //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 647 //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 7:7043da244e4b 648 //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 649 while(true)
pinofal 7:7043da244e4b 650 {
pinofal 7:7043da244e4b 651 // Vai avanti Anteriore
pinofal 7:7043da244e4b 652 PostOutBI1 = 0;
pinofal 7:7043da244e4b 653 PostOutBI2 = 0;
pinofal 7:7043da244e4b 654 AntOutBI1 = 1;
pinofal 7:7043da244e4b 655 AntOutBI2 = 0;
pinofal 7:7043da244e4b 656 PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 657 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 658 pc.printf("Avanti Anteriore\r\n");
pinofal 7:7043da244e4b 659 wait (2);
pinofal 7:7043da244e4b 660
pinofal 7:7043da244e4b 661 // spegni
pinofal 7:7043da244e4b 662 PostOutBI1 = 0;
pinofal 7:7043da244e4b 663 PostOutBI2 = 0;
pinofal 7:7043da244e4b 664 AntOutBI1 = 0;
pinofal 7:7043da244e4b 665 AntOutBI2 = 0;
pinofal 7:7043da244e4b 666 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 667 wait (3);
pinofal 7:7043da244e4b 668
pinofal 7:7043da244e4b 669 // Vai Indietro Anteriore
pinofal 7:7043da244e4b 670 PostOutBI1 = 0;
pinofal 7:7043da244e4b 671 PostOutBI2 = 0;
pinofal 7:7043da244e4b 672 AntOutBI1 = 0;
pinofal 7:7043da244e4b 673 AntOutBI2 = 1;
pinofal 7:7043da244e4b 674 PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 675 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 676 pc.printf("Indietro Anteriore \r\n");
pinofal 7:7043da244e4b 677 wait (2);
pinofal 7:7043da244e4b 678
pinofal 7:7043da244e4b 679 // spegni
pinofal 7:7043da244e4b 680 PostOutBI1 = 0;
pinofal 7:7043da244e4b 681 PostOutBI2 = 0;
pinofal 7:7043da244e4b 682 AntOutBI1 = 0;
pinofal 7:7043da244e4b 683 AntOutBI2 = 0;
pinofal 7:7043da244e4b 684 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 685 wait (3);
pinofal 7:7043da244e4b 686
pinofal 7:7043da244e4b 687 // Vai avanti Posteriore
pinofal 7:7043da244e4b 688 PostOutBI1 = 1;
pinofal 7:7043da244e4b 689 PostOutBI2 = 0;
pinofal 7:7043da244e4b 690 AntOutBI1 = 0;
pinofal 7:7043da244e4b 691 AntOutBI2 = 0;
pinofal 7:7043da244e4b 692 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 693 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 694 pc.printf("Avanti Posteriore\r\n");
pinofal 7:7043da244e4b 695 wait (2);
pinofal 7:7043da244e4b 696
pinofal 7:7043da244e4b 697 // spegni
pinofal 7:7043da244e4b 698 PostOutBI1 = 0;
pinofal 7:7043da244e4b 699 PostOutBI2 = 0;
pinofal 7:7043da244e4b 700 AntOutBI1 = 0;
pinofal 7:7043da244e4b 701 AntOutBI2 = 0;
pinofal 7:7043da244e4b 702 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 703 wait (3);
pinofal 7:7043da244e4b 704
pinofal 7:7043da244e4b 705 // Vai Indietro Posteriore
pinofal 7:7043da244e4b 706 PostOutBI1 = 0;
pinofal 7:7043da244e4b 707 PostOutBI2 = 1;
pinofal 7:7043da244e4b 708 AntOutBI1 = 0;
pinofal 7:7043da244e4b 709 AntOutBI2 = 0;
pinofal 7:7043da244e4b 710 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 711 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 712 pc.printf("Indietro Posteriore \r\n");
pinofal 7:7043da244e4b 713 wait (2);
pinofal 7:7043da244e4b 714
pinofal 7:7043da244e4b 715 // spegni
pinofal 7:7043da244e4b 716 PostOutBI1 = 0;
pinofal 7:7043da244e4b 717 PostOutBI2 = 0;
pinofal 7:7043da244e4b 718 AntOutBI1 = 0;
pinofal 7:7043da244e4b 719 AntOutBI2 = 0;
pinofal 7:7043da244e4b 720 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 721 wait (3);
pinofal 7:7043da244e4b 722
pinofal 7:7043da244e4b 723 // Vai avanti Anteriore + Posteriore
pinofal 7:7043da244e4b 724 PostOutBI1 = 1;
pinofal 7:7043da244e4b 725 PostOutBI2 = 0;
pinofal 7:7043da244e4b 726 AntOutBI1 = 1;
pinofal 7:7043da244e4b 727 AntOutBI2 = 0;
pinofal 7:7043da244e4b 728 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 729 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 730 pc.printf("Avanti Anteriore + Posteriore\r\n");
pinofal 7:7043da244e4b 731 wait (2);
pinofal 7:7043da244e4b 732
pinofal 7:7043da244e4b 733 // spegni
pinofal 7:7043da244e4b 734 PostOutBI1 = 0;
pinofal 7:7043da244e4b 735 PostOutBI2 = 0;
pinofal 7:7043da244e4b 736 AntOutBI1 = 0;
pinofal 7:7043da244e4b 737 AntOutBI2 = 0;
pinofal 7:7043da244e4b 738 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 739 wait (3);
pinofal 7:7043da244e4b 740
pinofal 7:7043da244e4b 741 // Vai Indietro Anteriore + Posteriore
pinofal 7:7043da244e4b 742 PostOutBI1 = 0;
pinofal 7:7043da244e4b 743 PostOutBI2 = 1;
pinofal 7:7043da244e4b 744 AntOutBI1 = 0;
pinofal 7:7043da244e4b 745 AntOutBI2 = 1;
pinofal 7:7043da244e4b 746 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 747 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 748 pc.printf("Indietro Anteriore + Posteriore \r\n");
pinofal 7:7043da244e4b 749 wait (2);
pinofal 7:7043da244e4b 750
pinofal 7:7043da244e4b 751 // spegni
pinofal 7:7043da244e4b 752 PostOutBI1 = 0;
pinofal 7:7043da244e4b 753 PostOutBI2 = 0;
pinofal 7:7043da244e4b 754 AntOutBI1 = 0;
pinofal 7:7043da244e4b 755 AntOutBI2 = 0;
pinofal 7:7043da244e4b 756 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 757 wait (3);
pinofal 7:7043da244e4b 758
pinofal 7:7043da244e4b 759 // Vai avanti Anteriore + Posteriore velocità ridotta
pinofal 7:7043da244e4b 760 PostOutBI1 = 1;
pinofal 7:7043da244e4b 761 PostOutBI2 = 0;
pinofal 7:7043da244e4b 762 AntOutBI1 = 1;
pinofal 7:7043da244e4b 763 AntOutBI2 = 0;
pinofal 7:7043da244e4b 764 PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 765 AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 766 pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
pinofal 7:7043da244e4b 767 wait (2);
pinofal 7:7043da244e4b 768
pinofal 7:7043da244e4b 769 // spegni
pinofal 7:7043da244e4b 770 PostOutBI1 = 0;
pinofal 7:7043da244e4b 771 PostOutBI2 = 0;
pinofal 7:7043da244e4b 772 AntOutBI1 = 0;
pinofal 7:7043da244e4b 773 AntOutBI2 = 0;
pinofal 7:7043da244e4b 774 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 775 wait (3);
pinofal 7:7043da244e4b 776
pinofal 7:7043da244e4b 777 // Vai Indietro Anteriore + Posteriore velocità ridotta
pinofal 7:7043da244e4b 778 PostOutBI1 = 0;
pinofal 7:7043da244e4b 779 PostOutBI2 = 1;
pinofal 7:7043da244e4b 780 AntOutBI1 = 0;
pinofal 7:7043da244e4b 781 AntOutBI2 = 1;
pinofal 7:7043da244e4b 782 PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 783 AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 784 pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
pinofal 7:7043da244e4b 785 wait (2);
pinofal 7:7043da244e4b 786
pinofal 7:7043da244e4b 787 // spegni
pinofal 7:7043da244e4b 788 PostOutBI1 = 0;
pinofal 7:7043da244e4b 789 PostOutBI2 = 0;
pinofal 7:7043da244e4b 790 AntOutBI1 = 0;
pinofal 7:7043da244e4b 791 AntOutBI2 = 0;
pinofal 7:7043da244e4b 792 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 793 wait (3);
pinofal 7:7043da244e4b 794
pinofal 7:7043da244e4b 795 // Ruota a destra
pinofal 7:7043da244e4b 796 PostOutBI1 = 0;
pinofal 7:7043da244e4b 797 PostOutBI2 = 1;
pinofal 7:7043da244e4b 798 AntOutBI1 = 1;
pinofal 7:7043da244e4b 799 AntOutBI2 = 0;
pinofal 7:7043da244e4b 800 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 801 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 802 pc.printf("Ruota a Destra\r\n");
pinofal 7:7043da244e4b 803 wait (2);
pinofal 7:7043da244e4b 804
pinofal 7:7043da244e4b 805 // spegni
pinofal 7:7043da244e4b 806 PostOutBI1 = 0;
pinofal 7:7043da244e4b 807 PostOutBI2 = 0;
pinofal 7:7043da244e4b 808 AntOutBI1 = 0;
pinofal 7:7043da244e4b 809 AntOutBI2 = 0;
pinofal 7:7043da244e4b 810 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 811 wait (3);
pinofal 7:7043da244e4b 812
pinofal 7:7043da244e4b 813 // Ruota a sinistra
pinofal 7:7043da244e4b 814 PostOutBI1 = 1;
pinofal 7:7043da244e4b 815 PostOutBI2 = 0;
pinofal 7:7043da244e4b 816 AntOutBI1 = 0;
pinofal 7:7043da244e4b 817 AntOutBI2 = 1;
pinofal 7:7043da244e4b 818 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 7:7043da244e4b 819 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 7:7043da244e4b 820 pc.printf("Ruota a Sinistra\r\n");
pinofal 7:7043da244e4b 821 wait (2);
pinofal 7:7043da244e4b 822
pinofal 7:7043da244e4b 823 // spegni
pinofal 7:7043da244e4b 824 PostOutBI1 = 0;
pinofal 7:7043da244e4b 825 PostOutBI2 = 0;
pinofal 7:7043da244e4b 826 AntOutBI1 = 0;
pinofal 7:7043da244e4b 827 AntOutBI2 = 0;
pinofal 7:7043da244e4b 828 pc.printf("Fermo\r\n\r\n");
pinofal 7:7043da244e4b 829 wait (3);
pinofal 7:7043da244e4b 830
pinofal 7:7043da244e4b 831 } // while(true) Test motore con PWM
pinofal 7:7043da244e4b 832 */
pinofal 7:7043da244e4b 833 //++++++++++++ FINE Test Motore con PWM ++++++++++++
pinofal 7:7043da244e4b 834
pinofal 7:7043da244e4b 835 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 836 //++++++++++++++ FINE Ciclo test +++++++++++++++++++
pinofal 7:7043da244e4b 837 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 7:7043da244e4b 838
pinofal 7:7043da244e4b 839 } // main()