Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev087

ultima versione funzionante per nuova app e watchdog

Dependencies:   mbed

MicRobot-Rev087.cpp@19:19892c59a33b, 2020-07-09 (annotated)

Committer:
maristella
Date:
Thu Jul 09 14:43:47 2020 +0000
Revision:
19:19892c59a33b
ultima versione funzionante per nuova app e watchdog

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maristella 19:19892c59a33b 1 // pilotaggio carrello tramite BLE.
maristella 19:19892c59a33b 2 // testato su L476RG e F401RE
maristella 19:19892c59a33b 3
maristella 19:19892c59a33b 4 #include "mbed.h"
maristella 19:19892c59a33b 5 #include<stdlib.h>
maristella 19:19892c59a33b 6
maristella 19:19892c59a33b 7 // attivare questa #define quando si vuole simulare l'arrivo di un segnale di encoder dai motori in movimento
maristella 19:19892c59a33b 8 //#define ENCODERSIMULATE
maristella 19:19892c59a33b 9
maristella 19:19892c59a33b 10 // pi greco
maristella 19:19892c59a33b 11 #define PI 3.14159265358979323846
maristella 19:19892c59a33b 12
maristella 19:19892c59a33b 13 // dimensione massima del pacchetto ricevuto su seriale
maristella 19:19892c59a33b 14 #define PACKETDIM 8
maristella 19:19892c59a33b 15
maristella 19:19892c59a33b 16 // diametro della ruota in [metri]
maristella 19:19892c59a33b 17 #define DIAMETRORUOTA (0.1)
maristella 19:19892c59a33b 18
maristella 19:19892c59a33b 19 // numero di impulsi per giro generati dall'encoder
maristella 19:19892c59a33b 20 #define IMPULSIPERGIRO 4
maristella 19:19892c59a33b 21
maristella 19:19892c59a33b 22 // numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m]
maristella 19:19892c59a33b 23 #define NUMCIFREDISTANZAPERCORSA 7
maristella 19:19892c59a33b 24
maristella 19:19892c59a33b 25 // 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]
maristella 19:19892c59a33b 26 #define NUMCIFRESPEED 7
maristella 19:19892c59a33b 27
maristella 19:19892c59a33b 28 // intervallo di tempo in [sec], in cui vengono contati gli impulsi di encoder per il calcolo della velocità
maristella 19:19892c59a33b 29 #define DELTAT (0.5)
maristella 19:19892c59a33b 30
maristella 19:19892c59a33b 31
maristella 19:19892c59a33b 32 // Parametri moltiplicativi. Queste operazioni vengono fatte una sola volta, evitando di farle ad ogni ciclo
maristella 19:19892c59a33b 33 #define fDistanzaPerStep (PI*DIAMETRORUOTA/IMPULSIPERGIRO)
maristella 19:19892c59a33b 34
maristella 19:19892c59a33b 35
maristella 19:19892c59a33b 36 // Ogni Ticker viene calcolata la velocità. Se il ticker viene richiamato ogni DELTAT sec, la velocità potrà essere calcolata come v = spazio/DELTAT
maristella 19:19892c59a33b 37 Ticker SpeedCalculateTicker;
maristella 19:19892c59a33b 38
maristella 19:19892c59a33b 39 //!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
maristella 19:19892c59a33b 40 #ifdef ENCODERSIMULATE
maristella 19:19892c59a33b 41 Ticker EncoderSimulateTicker; // Ticker per simulare un segnale proveniente da encoder sul motore
maristella 19:19892c59a33b 42 #endif
maristella 19:19892c59a33b 43 //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
maristella 19:19892c59a33b 44
maristella 19:19892c59a33b 45 // Definizione periferica USB seriale del PC
maristella 19:19892c59a33b 46 Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
maristella 19:19892c59a33b 47
maristella 19:19892c59a33b 48 // Definizione periferica seriale del Modulo BLE ELETT114A
maristella 19:19892c59a33b 49 Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps // F401
maristella 19:19892c59a33b 50 //Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
maristella 19:19892c59a33b 51
maristella 19:19892c59a33b 52 // Input di Reset per il Modulo BLE HC-05
maristella 19:19892c59a33b 53 DigitalOut BleRst(PA_8);
maristella 19:19892c59a33b 54
maristella 19:19892c59a33b 55 // User Button, LED
maristella 19:19892c59a33b 56 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
maristella 19:19892c59a33b 57 DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
maristella 19:19892c59a33b 58
maristella 19:19892c59a33b 59 // output digitale per pilotaggio illuminazione a LED
maristella 19:19892c59a33b 60 DigitalOut Light(PA_0);
maristella 19:19892c59a33b 61 //DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20
maristella 19:19892c59a33b 62 InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore.
maristella 19:19892c59a33b 63
maristella 19:19892c59a33b 64 // variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot
maristella 19:19892c59a33b 65 volatile int nCountRiseEdge;
maristella 19:19892c59a33b 66 volatile int nOldCountRiseEdge;
maristella 19:19892c59a33b 67
maristella 19:19892c59a33b 68 // Input/Output
maristella 19:19892c59a33b 69 DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
maristella 19:19892c59a33b 70 PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore
maristella 19:19892c59a33b 71 //DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
maristella 19:19892c59a33b 72 DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
maristella 19:19892c59a33b 73 DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
maristella 19:19892c59a33b 74
maristella 19:19892c59a33b 75 DigitalOut AntOutBI1 (PB_3); // Output 1 per pilotaggio input BI1 del Motore B Anteriore
maristella 19:19892c59a33b 76 PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore
maristella 19:19892c59a33b 77 //DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
maristella 19:19892c59a33b 78 DigitalOut AntOutBI2 (PB_4); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
maristella 19:19892c59a33b 79 DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
maristella 19:19892c59a33b 80 DigitalOut WatchDog (PB_0); // output WatchDog che deve oscillare con un periodo t < 1.5 s
maristella 19:19892c59a33b 81 PwmOut MotoreCoda (PB_8); // Output movimento coda
maristella 19:19892c59a33b 82
maristella 19:19892c59a33b 83
maristella 19:19892c59a33b 84 //carattere di comando ricevuto dal BLE e relativo parametro
maristella 19:19892c59a33b 85 volatile char cCommandBLE; // cambia nella routine di interrupt
maristella 19:19892c59a33b 86 volatile char cParamBLE; // cambia nella routine di interrupt
maristella 19:19892c59a33b 87 volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
maristella 19:19892c59a33b 88
maristella 19:19892c59a33b 89 // memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
maristella 19:19892c59a33b 90 char cOldCommandBLE;
maristella 19:19892c59a33b 91 int nOldParamBLE;
maristella 19:19892c59a33b 92
maristella 19:19892c59a33b 93 // coordinate polari del joystick sulla APP, fornite dalla routine di interrupt
maristella 19:19892c59a33b 94 volatile double fTeta;
maristella 19:19892c59a33b 95 volatile double fRo;
maristella 19:19892c59a33b 96 volatile int nRo;
maristella 19:19892c59a33b 97 volatile int nTeta;
maristella 19:19892c59a33b 98
maristella 19:19892c59a33b 99 // coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
maristella 19:19892c59a33b 100 volatile double fX, fY;
maristella 19:19892c59a33b 101 // memorizza ultimi valori delle coordinate del Joystick
maristella 19:19892c59a33b 102 double fOldX, fOldY;
maristella 19:19892c59a33b 103
maristella 19:19892c59a33b 104 // variabili ausiliarie per l'algoritmo di posizionamento
maristella 19:19892c59a33b 105 double fV, fW;
maristella 19:19892c59a33b 106
maristella 19:19892c59a33b 107 // velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
maristella 19:19892c59a33b 108 double fR, fL;
maristella 19:19892c59a33b 109
maristella 19:19892c59a33b 110 // distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore
maristella 19:19892c59a33b 111 volatile double fDistanzaPercorsa; // calcolata nel main, utilizzata nelle IRQ
maristella 19:19892c59a33b 112
maristella 19:19892c59a33b 113 // velocità calcolata gli impulsi contati in un intervallo DELTAT msec
maristella 19:19892c59a33b 114 volatile double fSpeed; // calcolata nel main, utilizzata nelle IRQ
maristella 19:19892c59a33b 115
maristella 19:19892c59a33b 116 // Scopi diagnostici: Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
maristella 19:19892c59a33b 117 // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
maristella 19:19892c59a33b 118 double fDeltaTick;
maristella 19:19892c59a33b 119
maristella 19:19892c59a33b 120 // indice per i cicli
maristella 19:19892c59a33b 121 int nIndex;
maristella 19:19892c59a33b 122
maristella 19:19892c59a33b 123 // esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
maristella 19:19892c59a33b 124 double fEsponente;
maristella 19:19892c59a33b 125
maristella 19:19892c59a33b 126 // array per la ricezione dei messaggi da BLE
maristella 19:19892c59a33b 127 volatile char caRxPacket[PACKETDIM];
maristella 19:19892c59a33b 128 // contatore di caratteri ricevuti daBLE
maristella 19:19892c59a33b 129 volatile int nCharCount;
maristella 19:19892c59a33b 130
maristella 19:19892c59a33b 131 // flag che indica se il sw è in Reset
maristella 19:19892c59a33b 132 volatile bool bReset;
maristella 19:19892c59a33b 133
maristella 19:19892c59a33b 134 // flag che indica se la coda è in movimento/ferma true/false
maristella 19:19892c59a33b 135 volatile bool bCodaInMovimento;
maristella 19:19892c59a33b 136
maristella 19:19892c59a33b 137 /**************************************************************************************/
maristella 19:19892c59a33b 138 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
maristella 19:19892c59a33b 139 /**************************************************************************************/
maristella 19:19892c59a33b 140 void riseEncoderIRQ()
maristella 19:19892c59a33b 141 {
maristella 19:19892c59a33b 142 // incrementa il contatore di impulsi contati, se il sw non è resettato, cioè se bReset = false
maristella 19:19892c59a33b 143 //if(!bReset)
maristella 19:19892c59a33b 144 nCountRiseEdge++;
maristella 19:19892c59a33b 145
maristella 19:19892c59a33b 146 //pc.printf("Sono qui 0 \n\r"); // diagnostica
maristella 19:19892c59a33b 147 }
maristella 19:19892c59a33b 148
maristella 19:19892c59a33b 149
maristella 19:19892c59a33b 150 /****************************************************************************************/
maristella 19:19892c59a33b 151 /* Diagnostica: */
maristella 19:19892c59a33b 152 /* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO */
maristella 19:19892c59a33b 153 /* Routine di gestione del ticker per simulare encoder */
maristella 19:19892c59a33b 154 /* Simula il segnale di encoder ricevuto con un determinato DELTAT */
maristella 19:19892c59a33b 155 /* A robot fermo, il segnale di encoder non genera interrupt. */
maristella 19:19892c59a33b 156 /* Questo Ticker simula l'arrivo del segnale da encoder */
maristella 19:19892c59a33b 157 /****************************************************************************************/
maristella 19:19892c59a33b 158 void EncoderSimulate()
maristella 19:19892c59a33b 159 {
maristella 19:19892c59a33b 160 // ad ogni tick viene simulata la ricezione di un impulso da encoder.
maristella 19:19892c59a33b 161 // Esempio:
maristella 19:19892c59a33b 162 // fDeltaTick = 0.05 sec
maristella 19:19892c59a33b 163 // diametro ruota, DIAMETRORUOTA = 0.1 metri
maristella 19:19892c59a33b 164 // circonferenza ruota = 0.1*3.14= 0.314 metri
maristella 19:19892c59a33b 165 // impulsi per giro dall'encoder, IMPULSIPERGIRO = 4
maristella 19:19892c59a33b 166 // un tick simula l'arrivo di un impulso da encoder e quindi simula la percorrenza di 1/4 di circonferenza
maristella 19:19892c59a33b 167 // ogni volta che arriva un tick simulato da encoder, si presume di aver percorso circonferenza/4 = 0.314/4 = 0.0785 metri
maristella 19:19892c59a33b 168 // il tick arriva ogni fDeltaTick secondi e a ogni tick percorro 0.0785 metri -> velocità = 0.0785/0.05 = 1.57 [m/s]
maristella 19:19892c59a33b 169 // spostamento = (Spazio per ogni tick)/(tempo per ogni tick)
maristella 19:19892c59a33b 170 // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
maristella 19:19892c59a33b 171
maristella 19:19892c59a33b 172 // simula impulso inviato dall'encoder se il sw non è resettato, cioè se bReset = false
maristella 19:19892c59a33b 173 //if(!bReset)
maristella 19:19892c59a33b 174 nCountRiseEdge++;
maristella 19:19892c59a33b 175 }
maristella 19:19892c59a33b 176
maristella 19:19892c59a33b 177
maristella 19:19892c59a33b 178 /**********************************************/
maristella 19:19892c59a33b 179 // IRQ associata a Rx da PC
maristella 19:19892c59a33b 180 //**********************************************/
maristella 19:19892c59a33b 181 void pcRxInterrupt(void)
maristella 19:19892c59a33b 182 {
maristella 19:19892c59a33b 183 // array per la ricezione dei messaggi da seriale
maristella 19:19892c59a33b 184 char cReadChar;
maristella 19:19892c59a33b 185
maristella 19:19892c59a33b 186 // ricevi caratteri su seriale, se disponibili
maristella 19:19892c59a33b 187 while((pc.readable()))
maristella 19:19892c59a33b 188 {
maristella 19:19892c59a33b 189 // acquisice stringa in input e relativa dimensione
maristella 19:19892c59a33b 190 cReadChar = pc.getc(); // read character from PC
maristella 19:19892c59a33b 191 //myBLE.putc(cReadChar); // Diagnostica: write char to BLE
maristella 19:19892c59a33b 192 //pc.putc(cReadChar); // Diagnostica: write char to PC
maristella 19:19892c59a33b 193
maristella 19:19892c59a33b 194 //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica
maristella 19:19892c59a33b 195 if(cReadChar == '0') // se scrivo '0', invia questa stringa
maristella 19:19892c59a33b 196 {
maristella 19:19892c59a33b 197 // DIAGNOSTICA:
maristella 19:19892c59a33b 198 // Invia Stringa di comando al Robot
maristella 19:19892c59a33b 199 myBLE.printf("*W\r\n> Prova di Trasmissione \r\n*");
maristella 19:19892c59a33b 200 }
maristella 19:19892c59a33b 201 }
maristella 19:19892c59a33b 202 }
maristella 19:19892c59a33b 203
maristella 19:19892c59a33b 204 //**********************************************/
maristella 19:19892c59a33b 205 // IRQ associata a Rx da BLE
maristella 19:19892c59a33b 206 //**********************************************/
maristella 19:19892c59a33b 207 void BLERxInterrupt(void)
maristella 19:19892c59a33b 208 {
maristella 19:19892c59a33b 209
maristella 19:19892c59a33b 210 // carattere ricevuto da BLE
maristella 19:19892c59a33b 211 char cReadChar;
maristella 19:19892c59a33b 212
maristella 19:19892c59a33b 213 while((myBLE.readable()))
maristella 19:19892c59a33b 214 {
maristella 19:19892c59a33b 215 // acquisice stringa in input e memorizza in array
maristella 19:19892c59a33b 216 cReadChar = myBLE.getc(); // Read character
maristella 19:19892c59a33b 217 //caRxPacket[nCharCount]=cReadChar;
maristella 19:19892c59a33b 218 //nCharCount++;
maristella 19:19892c59a33b 219 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 220
maristella 19:19892c59a33b 221 // acquisisce il carattere di start comando o coordinate da APP
maristella 19:19892c59a33b 222 if(cReadChar=='(')
maristella 19:19892c59a33b 223 {
maristella 19:19892c59a33b 224 // acquisisce il primo carattere di comando o di coordinate
maristella 19:19892c59a33b 225 cReadChar = myBLE.getc(); // Read character
maristella 19:19892c59a33b 226 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 227
maristella 19:19892c59a33b 228 // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
maristella 19:19892c59a33b 229 // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T
maristella 19:19892c59a33b 230 if((cReadChar > 0x40) && (cReadChar < 0x55)) // caratteri alfabetici da 'A' a 'T'
maristella 19:19892c59a33b 231 {
maristella 19:19892c59a33b 232 // memorizza come comando il carattere appena letto
maristella 19:19892c59a33b 233 cCommandBLE = cReadChar;
maristella 19:19892c59a33b 234 // legge e memorizza come paramentro il successivo carattere
maristella 19:19892c59a33b 235 cReadChar = myBLE.getc(); // legge parametro
maristella 19:19892c59a33b 236 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 237 nParamBLE = cReadChar-0x30;
maristella 19:19892c59a33b 238 cReadChar = myBLE.getc(); // legge la ')' di chiusura comando
maristella 19:19892c59a33b 239 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 240
maristella 19:19892c59a33b 241 // visualizza comando e parametro inviato da BLE
maristella 19:19892c59a33b 242 pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
maristella 19:19892c59a33b 243 }
maristella 19:19892c59a33b 244 // +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
maristella 19:19892c59a33b 245 else
maristella 19:19892c59a33b 246 {
maristella 19:19892c59a33b 247 if(cReadChar=='X') // è stato acquisisto carattere X di inizio valori numeri dell'ascissa?
maristella 19:19892c59a33b 248 {
maristella 19:19892c59a33b 249 //+++++++++ INIZIO acquisisce il valore dell'ascissa Xnnn +++++++++++++++
maristella 19:19892c59a33b 250 nCharCount = 0;
maristella 19:19892c59a33b 251 do
maristella 19:19892c59a33b 252 {
maristella 19:19892c59a33b 253 cReadChar = myBLE.getc(); // Read character
maristella 19:19892c59a33b 254 caRxPacket[nCharCount]=cReadChar;
maristella 19:19892c59a33b 255 nCharCount++;
maristella 19:19892c59a33b 256 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 257 }
maristella 19:19892c59a33b 258 while((cReadChar >= 0x30) && (cReadChar <= 0x39));
maristella 19:19892c59a33b 259 //+++++++++ FINE acquisisce il valore dell'ascissa Xnnn +++++++++++++++
maristella 19:19892c59a33b 260
maristella 19:19892c59a33b 261 //+++++++++ INIZIO converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
maristella 19:19892c59a33b 262 nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
maristella 19:19892c59a33b 263 fX=0;
maristella 19:19892c59a33b 264 for(nIndex =0; nIndex <= nCharCount; nIndex++)
maristella 19:19892c59a33b 265 {
maristella 19:19892c59a33b 266 fX = fX + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
maristella 19:19892c59a33b 267 }
maristella 19:19892c59a33b 268 //+++++++++ FINE converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
maristella 19:19892c59a33b 269
maristella 19:19892c59a33b 270 // verifica se l'ultimo carattere ricevuto dopo Xnnn è stato Y
maristella 19:19892c59a33b 271 if(cReadChar=='Y')
maristella 19:19892c59a33b 272 {
maristella 19:19892c59a33b 273 //+++++++++ INIZIO acquisisce il valore dell'ordinata Ynnn +++++++++++++++
maristella 19:19892c59a33b 274 nCharCount = 0;
maristella 19:19892c59a33b 275 do
maristella 19:19892c59a33b 276 {
maristella 19:19892c59a33b 277 cReadChar = myBLE.getc(); // Read character
maristella 19:19892c59a33b 278 caRxPacket[nCharCount]=cReadChar;
maristella 19:19892c59a33b 279 nCharCount++;
maristella 19:19892c59a33b 280 pc.printf("%c", cReadChar); // diagnostica
maristella 19:19892c59a33b 281 }
maristella 19:19892c59a33b 282 while((cReadChar >= 0x30) && (cReadChar <= 0x39));
maristella 19:19892c59a33b 283 //+++++++++ FINE acquisisce il valore dell'ordinata Ynnn +++++++++++++++
maristella 19:19892c59a33b 284
maristella 19:19892c59a33b 285 //+++++++++ INIZIO converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
maristella 19:19892c59a33b 286 nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
maristella 19:19892c59a33b 287 fY=0;
maristella 19:19892c59a33b 288 for(nIndex =0; nIndex <= nCharCount; nIndex++)
maristella 19:19892c59a33b 289 {
maristella 19:19892c59a33b 290 fY = fY + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
maristella 19:19892c59a33b 291 }
maristella 19:19892c59a33b 292 //+++++++++ FINE converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
maristella 19:19892c59a33b 293
maristella 19:19892c59a33b 294 // se riceve la coda del comando ), stampa le coordinate ricevute
maristella 19:19892c59a33b 295 if(cReadChar==')')
maristella 19:19892c59a33b 296 {
maristella 19:19892c59a33b 297 pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica
maristella 19:19892c59a33b 298
maristella 19:19892c59a33b 299 // trasporta x e y nei range desiderati
maristella 19:19892c59a33b 300 fY= 100 - fY;
maristella 19:19892c59a33b 301 fX = fX - 100;
maristella 19:19892c59a33b 302
maristella 19:19892c59a33b 303 pc.printf("\n\r(fX , fY) traslate = (%.1f , %.1f) \r\n\n", fX, fY); // diagnostica
maristella 19:19892c59a33b 304 }
maristella 19:19892c59a33b 305 else // dopo la Y e i numeri, mi attendo parentesi chiusa )
maristella 19:19892c59a33b 306 {
maristella 19:19892c59a33b 307 pc.printf("> Errore = %c invece di ) \r\n\n", cReadChar); // diagnostica
maristella 19:19892c59a33b 308 }
maristella 19:19892c59a33b 309 }
maristella 19:19892c59a33b 310 else // dopo la X e i numeri mi aspetto Y
maristella 19:19892c59a33b 311 {
maristella 19:19892c59a33b 312 pc.printf("> Errore = %c invece di Y \r\n\n", cReadChar); // diagnostica
maristella 19:19892c59a33b 313 }
maristella 19:19892c59a33b 314 }
maristella 19:19892c59a33b 315 else // dopo la ( mi aspetto Y
maristella 19:19892c59a33b 316 {
maristella 19:19892c59a33b 317 pc.printf("> Errore = %c invece di X \r\n\n", cReadChar); // diagnostica
maristella 19:19892c59a33b 318 }
maristella 19:19892c59a33b 319 } // if(comandi alfanumerici)
maristella 19:19892c59a33b 320 } // if(cReadChar == '(')
maristella 19:19892c59a33b 321 }
maristella 19:19892c59a33b 322 }
maristella 19:19892c59a33b 323
maristella 19:19892c59a33b 324 /*********************************************************************************************************************************************/
maristella 19:19892c59a33b 325 /* ogni DELTAT secondi scatta questo ticker. */
maristella 19:19892c59a33b 326 /* Tra due Tick viene contato il numero di mpulsi impulsi di encoder ricevuti con degli interrupt e contentuo nella variabile nCountRiseEdge */
maristella 19:19892c59a33b 327 /*********************************************************************************************************************************************/
maristella 19:19892c59a33b 328 void SpeedCalculate()
maristella 19:19892c59a33b 329 {
maristella 19:19892c59a33b 330
maristella 19:19892c59a33b 331 // se bReset = true non fare nessun calcolo della velocità e spostamento e azzera velocità e spostamento
maristella 19:19892c59a33b 332 if(!bReset)
maristella 19:19892c59a33b 333 {
maristella 19:19892c59a33b 334 //pc.printf("Sono qui 1 \n\r"); // diagnostica
maristella 19:19892c59a33b 335 //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 336 //nCountRiseEdge++; //----diagnostica
maristella 19:19892c59a33b 337 // se nella IRQ, durante il periodo di calcolo della velocità, sono stati contati fronti di salita dell'encoder, il robot si sta muovendo
maristella 19:19892c59a33b 338 if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo
maristella 19:19892c59a33b 339 {
maristella 19:19892c59a33b 340 //pc.printf("nCountRiseEdge= %d ; nOldCountRiseEdge= %d \n\r", nCountRiseEdge, nOldCountRiseEdge); // diagnostica
maristella 19:19892c59a33b 341
maristella 19:19892c59a33b 342 // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
maristella 19:19892c59a33b 343 fDistanzaPercorsa = fDistanzaPerStep*nCountRiseEdge;
maristella 19:19892c59a33b 344
maristella 19:19892c59a33b 345 // calcola la velocità in [m/sec]. DELTAT è in [sec] lo spostamento è in [m]
maristella 19:19892c59a33b 346 //fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT);
maristella 19:19892c59a33b 347 fSpeed = (fDistanzaPerStep*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT;
maristella 19:19892c59a33b 348
maristella 19:19892c59a33b 349 // ricorda lo spostamento
maristella 19:19892c59a33b 350 nOldCountRiseEdge = nCountRiseEdge;
maristella 19:19892c59a33b 351
maristella 19:19892c59a33b 352 // comunica al cellulare vleocità e spostamento mentre si sta spostando
maristella 19:19892c59a33b 353 //PRIMA ERA QUI ma si bloccava myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa );
maristella 19:19892c59a33b 354 }
maristella 19:19892c59a33b 355 else
maristella 19:19892c59a33b 356 {
maristella 19:19892c59a33b 357 // se non ci sono variazioni di impulsi, il robot è fermo, la velocità è 0.0
maristella 19:19892c59a33b 358 fSpeed= 0.0;
maristella 19:19892c59a33b 359 }
maristella 19:19892c59a33b 360
maristella 19:19892c59a33b 361 //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); // diagnostica
maristella 19:19892c59a33b 362 }
maristella 19:19892c59a33b 363 else
maristella 19:19892c59a33b 364 {
maristella 19:19892c59a33b 365 // bReset = true
maristella 19:19892c59a33b 366 // comunica al cellulare vleocità e spostamento nulli
maristella 19:19892c59a33b 367 nOldCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
maristella 19:19892c59a33b 368 nCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
maristella 19:19892c59a33b 369 fSpeed =0.0;
maristella 19:19892c59a33b 370 fDistanzaPercorsa = 0.0;
maristella 19:19892c59a33b 371 myBLE.printf("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa ); //*W=carattere di riconoscimento per comunicazione con APP (inviare messaggio)
maristella 19:19892c59a33b 372 }
maristella 19:19892c59a33b 373 // stimola il watchdog esterno facendo oscillare la linea WatchDog
maristella 19:19892c59a33b 374 if(WatchDog == 0)
maristella 19:19892c59a33b 375 {
maristella 19:19892c59a33b 376 WatchDog = 1;
maristella 19:19892c59a33b 377 pc.printf("WatchDog = 1\n\r");
maristella 19:19892c59a33b 378 }
maristella 19:19892c59a33b 379 else
maristella 19:19892c59a33b 380 {
maristella 19:19892c59a33b 381 WatchDog = 0;
maristella 19:19892c59a33b 382 pc.printf("WatchDog = 0\n\r");
maristella 19:19892c59a33b 383 }
maristella 19:19892c59a33b 384 //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 385 }
maristella 19:19892c59a33b 386
maristella 19:19892c59a33b 387
maristella 19:19892c59a33b 388
maristella 19:19892c59a33b 389 /**********/
maristella 19:19892c59a33b 390 /* MAIN */
maristella 19:19892c59a33b 391 /**********/
maristella 19:19892c59a33b 392 int main()
maristella 19:19892c59a33b 393 {
maristella 19:19892c59a33b 394 // inizializza PWM del motore coda
maristella 19:19892c59a33b 395 MotoreCoda.period_ms(50); // periodo PWM
maristella 19:19892c59a33b 396 bCodaInMovimento = false;
maristella 19:19892c59a33b 397
maristella 19:19892c59a33b 398 // messaggio di benvenuto
maristella 19:19892c59a33b 399 pc.printf("\r\n************ Hallo ****************** \r\n");
maristella 19:19892c59a33b 400 pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
maristella 19:19892c59a33b 401 myBLE.printf("*W \r\nHallo\r\n*");
maristella 19:19892c59a33b 402 myBLE.printf("*W Modulo di Ispezione Condutture \r\n*");
maristella 19:19892c59a33b 403
maristella 19:19892c59a33b 404 // inizializza variabili da BLE
maristella 19:19892c59a33b 405 cCommandBLE = 0; // inizialmente nessun comando da BLE
maristella 19:19892c59a33b 406 cOldCommandBLE = 0; // inizialmente nessun comando da BLE
maristella 19:19892c59a33b 407 cParamBLE = 0; // inizialmente nessun parametro da BLE
maristella 19:19892c59a33b 408 nParamBLE=0; // inizialmente nessun parametro da BLE
maristella 19:19892c59a33b 409 nOldParamBLE=0; // inizialmente nessun parametro da BLE
maristella 19:19892c59a33b 410 fX = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
maristella 19:19892c59a33b 411 fOldX = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
maristella 19:19892c59a33b 412 fY = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
maristella 19:19892c59a33b 413 fOldY = 0; // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
maristella 19:19892c59a33b 414 bReset = false; //bReset = true/false quando riceve un comando (R1)/(R0) dalla APP
maristella 19:19892c59a33b 415 WatchDog = 0; // inizialmente watchdog = 0; oscillerà per non ricevere reset esterno
maristella 19:19892c59a33b 416
maristella 19:19892c59a33b 417 // inizializza variabili
maristella 19:19892c59a33b 418 fDistanzaPercorsa = 0.0;
maristella 19:19892c59a33b 419 fSpeed = 0.0;
maristella 19:19892c59a33b 420
maristella 19:19892c59a33b 421 // inizializza array di caratteri ricevuti
maristella 19:19892c59a33b 422 for(nIndex=0; nIndex < PACKETDIM; nIndex++)
maristella 19:19892c59a33b 423 {caRxPacket[nIndex]=0;}
maristella 19:19892c59a33b 424 nCharCount=0;
maristella 19:19892c59a33b 425
maristella 19:19892c59a33b 426
maristella 19:19892c59a33b 427 // inizializza i valori di modulo e fase ricevuti dal joystick
maristella 19:19892c59a33b 428 nRo = 0;
maristella 19:19892c59a33b 429 nTeta = 0;
maristella 19:19892c59a33b 430
maristella 19:19892c59a33b 431 //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 432 // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
maristella 19:19892c59a33b 433 // gli impulsi di encoder vengono contati da una IRQ collegata all'input da encoder
maristella 19:19892c59a33b 434 // ogni DELTAT secondi scatta un ticker che calcola la velocità
maristella 19:19892c59a33b 435
maristella 19:19892c59a33b 436 // definisci il mode del segnale digitale di EncoderA
maristella 19:19892c59a33b 437 InEncoderA.mode(PullUp);
maristella 19:19892c59a33b 438
maristella 19:19892c59a33b 439 // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
maristella 19:19892c59a33b 440 InEncoderA.rise(&riseEncoderIRQ);
maristella 19:19892c59a33b 441 // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
maristella 19:19892c59a33b 442 nCountRiseEdge=0;
maristella 19:19892c59a33b 443 nOldCountRiseEdge=0;
maristella 19:19892c59a33b 444
maristella 19:19892c59a33b 445 InEncoderA.enable_irq();
maristella 19:19892c59a33b 446 SpeedCalculateTicker.attach(&SpeedCalculate,DELTAT);
maristella 19:19892c59a33b 447 //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 448
maristella 19:19892c59a33b 449 // Attiva la IRQ per la RX su seriale e sulla Rx della BLE
maristella 19:19892c59a33b 450 myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla seriale del BLE
maristella 19:19892c59a33b 451 pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
maristella 19:19892c59a33b 452
maristella 19:19892c59a33b 453 // attiva un ticker per simulare robot in movimento.
maristella 19:19892c59a33b 454 //!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
maristella 19:19892c59a33b 455 #ifdef ENCODERSIMULATE
maristella 19:19892c59a33b 456 // attiva il Ticker per simulare il calcolo della velocità. Ogni fDeltaTick viene simulato l'arrivo di un impulso dall'encoder del motore
maristella 19:19892c59a33b 457 fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
maristella 19:19892c59a33b 458 EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
maristella 19:19892c59a33b 459 #endif
maristella 19:19892c59a33b 460 //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!!
maristella 19:19892c59a33b 461
maristella 19:19892c59a33b 462 //++++++++++++++++++++++++++++++ INIZIO Test WatchDog +++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 463 /* while(true)
maristella 19:19892c59a33b 464 {
maristella 19:19892c59a33b 465 if(WatchDog == 0)
maristella 19:19892c59a33b 466 {
maristella 19:19892c59a33b 467 WatchDog = 1;
maristella 19:19892c59a33b 468 pc.printf("WatchDog = 1\n\r");
maristella 19:19892c59a33b 469 }
maristella 19:19892c59a33b 470 else
maristella 19:19892c59a33b 471 {
maristella 19:19892c59a33b 472 WatchDog = 0;
maristella 19:19892c59a33b 473 pc.printf("WatchDog = 0\n\r");
maristella 19:19892c59a33b 474 }
maristella 19:19892c59a33b 475
maristella 19:19892c59a33b 476 wait_ms(100);
maristella 19:19892c59a33b 477
maristella 19:19892c59a33b 478 }
maristella 19:19892c59a33b 479 */
maristella 19:19892c59a33b 480 //++++++++++++++++++++++++++++++ FINE Test WatchDog +++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 481
maristella 19:19892c59a33b 482 //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 483 /*
maristella 19:19892c59a33b 484 //pc.printf("Coda in movimento \n\r"); //diagnostica
maristella 19:19892c59a33b 485 while(true)
maristella 19:19892c59a33b 486 {
maristella 19:19892c59a33b 487
maristella 19:19892c59a33b 488 if ( ( fX != 0 ) || (fY != 0))
maristella 19:19892c59a33b 489 {
maristella 19:19892c59a33b 490 if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
maristella 19:19892c59a33b 491 {
maristella 19:19892c59a33b 492 MotoreCoda.write (0.6);
maristella 19:19892c59a33b 493 bCodaInMovimento = true;
maristella 19:19892c59a33b 494 pc.printf("Coda in movimento \n\r");
maristella 19:19892c59a33b 495 }
maristella 19:19892c59a33b 496 }
maristella 19:19892c59a33b 497 else
maristella 19:19892c59a33b 498 {
maristella 19:19892c59a33b 499 // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
maristella 19:19892c59a33b 500 if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
maristella 19:19892c59a33b 501 {
maristella 19:19892c59a33b 502 pc.printf("Coda ferma \n\r");
maristella 19:19892c59a33b 503 MotoreCoda.write (0.0);
maristella 19:19892c59a33b 504 bCodaInMovimento = false;
maristella 19:19892c59a33b 505 // comunica al cellulare vleocità nulla
maristella 19:19892c59a33b 506 // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE
maristella 19:19892c59a33b 507 //NVIC_DisableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 508 myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r", 100.0, 1000 );
maristella 19:19892c59a33b 509 //NVIC_EnableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 510
maristella 19:19892c59a33b 511 }
maristella 19:19892c59a33b 512 }
maristella 19:19892c59a33b 513
maristella 19:19892c59a33b 514 }
maristella 19:19892c59a33b 515 */
maristella 19:19892c59a33b 516 //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 517
maristella 19:19892c59a33b 518
maristella 19:19892c59a33b 519 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 520 //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++
maristella 19:19892c59a33b 521 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 522
maristella 19:19892c59a33b 523 while(true)
maristella 19:19892c59a33b 524 {
maristella 19:19892c59a33b 525
maristella 19:19892c59a33b 526 //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 527
maristella 19:19892c59a33b 528 if ((fX!=0) || (fY!=0)) //la coda non si muove se il Joystick è nella posizione (0,0)
maristella 19:19892c59a33b 529 {
maristella 19:19892c59a33b 530 // il joystick è in posizione diversa da (0,0), fai muovere la coda
maristella 19:19892c59a33b 531 if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
maristella 19:19892c59a33b 532 {
maristella 19:19892c59a33b 533 //pc.printf("Coda in movimento \n\r"); //diagnostica
maristella 19:19892c59a33b 534 MotoreCoda.write (0.4);
maristella 19:19892c59a33b 535 bCodaInMovimento = true;
maristella 19:19892c59a33b 536
maristella 19:19892c59a33b 537 }
maristella 19:19892c59a33b 538 }
maristella 19:19892c59a33b 539 else
maristella 19:19892c59a33b 540 {
maristella 19:19892c59a33b 541 // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
maristella 19:19892c59a33b 542 if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
maristella 19:19892c59a33b 543 {
maristella 19:19892c59a33b 544 //pc.printf("Coda ferma \n\r"); //diagnostica
maristella 19:19892c59a33b 545 MotoreCoda.write (0.0);
maristella 19:19892c59a33b 546 bCodaInMovimento = false;
maristella 19:19892c59a33b 547 // comunica al cellulare vleocità nulla
maristella 19:19892c59a33b 548 // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE
maristella 19:19892c59a33b 549 //NVIC_DisableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 550 //myBLE.printf("Speed= 0.0 [m/s]; Trip= %.2f [m]\n\r", fDistanzaPercorsa );
maristella 19:19892c59a33b 551 //NVIC_EnableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 552
maristella 19:19892c59a33b 553 }
maristella 19:19892c59a33b 554 }
maristella 19:19892c59a33b 555
maristella 19:19892c59a33b 556 if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
maristella 19:19892c59a33b 557 {
maristella 19:19892c59a33b 558 switch (cCommandBLE)
maristella 19:19892c59a33b 559 {
maristella 19:19892c59a33b 560 case 'T': // accendi/spegni LED su scheda
maristella 19:19892c59a33b 561 {
maristella 19:19892c59a33b 562 myLed = nParamBLE;
maristella 19:19892c59a33b 563 } break;
maristella 19:19892c59a33b 564 case 'L': // Accendi/spegni illuminazione a LED
maristella 19:19892c59a33b 565 {
maristella 19:19892c59a33b 566 Light = nParamBLE;
maristella 19:19892c59a33b 567 /* if (nParamBLE == 1)
maristella 19:19892c59a33b 568 {
maristella 19:19892c59a33b 569 myBLE.printf("*lR250G0B100*");
maristella 19:19892c59a33b 570 }
maristella 19:19892c59a33b 571 else
maristella 19:19892c59a33b 572 {
maristella 19:19892c59a33b 573 myBLE.printf("*lR0G0B0*");
maristella 19:19892c59a33b 574 }*/
maristella 19:19892c59a33b 575 } break;
maristella 19:19892c59a33b 576 case 'R': // Reset odometria e illuminazione
maristella 19:19892c59a33b 577 {
maristella 19:19892c59a33b 578 if(nParamBLE==1)
maristella 19:19892c59a33b 579 {
maristella 19:19892c59a33b 580 bReset = true;
maristella 19:19892c59a33b 581 nCountRiseEdge = 0;
maristella 19:19892c59a33b 582 nOldCountRiseEdge = 0;
maristella 19:19892c59a33b 583 Light = 0;
maristella 19:19892c59a33b 584 fDistanzaPercorsa = 0.0;
maristella 19:19892c59a33b 585 fSpeed = 0.0;
maristella 19:19892c59a33b 586 }
maristella 19:19892c59a33b 587 else
maristella 19:19892c59a33b 588 {
maristella 19:19892c59a33b 589 // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
maristella 19:19892c59a33b 590 bReset = false;
maristella 19:19892c59a33b 591 }
maristella 19:19892c59a33b 592 } break;
maristella 19:19892c59a33b 593
maristella 19:19892c59a33b 594 default: break;
maristella 19:19892c59a33b 595 }
maristella 19:19892c59a33b 596 //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
maristella 19:19892c59a33b 597 cOldCommandBLE = cCommandBLE;
maristella 19:19892c59a33b 598 nOldParamBLE = nParamBLE;
maristella 19:19892c59a33b 599 }
maristella 19:19892c59a33b 600
maristella 19:19892c59a33b 601 //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 602
maristella 19:19892c59a33b 603 //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++
maristella 19:19892c59a33b 604 //Invert X
maristella 19:19892c59a33b 605 //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
maristella 19:19892c59a33b 606 //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
maristella 19:19892c59a33b 607 //Calcola R: R = (V+W) /2
maristella 19:19892c59a33b 608 //Calcola L: L= (V-W)/2
maristella 19:19892c59a33b 609 //Scala i valori di L e R in base all'hardware.
maristella 19:19892c59a33b 610 //invia i valori al robot.
maristella 19:19892c59a33b 611 // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote
maristella 19:19892c59a33b 612 if( (fX != fOldX) || (fY != fOldY))
maristella 19:19892c59a33b 613 {
maristella 19:19892c59a33b 614 fOldX = fX;
maristella 19:19892c59a33b 615 fOldY = fY;
maristella 19:19892c59a33b 616 // algoritmo di conversione dalla posizione del Joystick (fX, fY) alla velocità delle ruote (fR, fL)
maristella 19:19892c59a33b 617 fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
maristella 19:19892c59a33b 618 fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
maristella 19:19892c59a33b 619 fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
maristella 19:19892c59a33b 620 fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
maristella 19:19892c59a33b 621 // diagnostica
maristella 19:19892c59a33b 622 //pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); // diagnostica
maristella 19:19892c59a33b 623 //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica
maristella 19:19892c59a33b 624 //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica
maristella 19:19892c59a33b 625 //pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); // diagnostica
maristella 19:19892c59a33b 626
maristella 19:19892c59a33b 627 // comunica al cellulare vleocità e spostamento mentre si sta spostando
maristella 19:19892c59a33b 628 // Attiva la IRQ per la RX su seriale e sulla Rx della BLE
maristella 19:19892c59a33b 629 NVIC_DisableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 630 myBLE.printf("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa );
maristella 19:19892c59a33b 631 NVIC_EnableIRQ(USART1_IRQn);
maristella 19:19892c59a33b 632
maristella 19:19892c59a33b 633 // algoritmo di movimentazione delle ruote.
maristella 19:19892c59a33b 634 if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
maristella 19:19892c59a33b 635 {
maristella 19:19892c59a33b 636 fR =-fR;
maristella 19:19892c59a33b 637 // Vai indietro
maristella 19:19892c59a33b 638 PostOutBI1 = 1;
maristella 19:19892c59a33b 639 PostOutBI2 = 0;
maristella 19:19892c59a33b 640 }
maristella 19:19892c59a33b 641 else
maristella 19:19892c59a33b 642 {
maristella 19:19892c59a33b 643 if(fR >0)
maristella 19:19892c59a33b 644 {
maristella 19:19892c59a33b 645 // Vai avanti
maristella 19:19892c59a33b 646 PostOutBI1 = 0;
maristella 19:19892c59a33b 647 PostOutBI2 = 1;
maristella 19:19892c59a33b 648 }
maristella 19:19892c59a33b 649 else
maristella 19:19892c59a33b 650 {
maristella 19:19892c59a33b 651 // spegni
maristella 19:19892c59a33b 652 PostOutBI1 = 0;
maristella 19:19892c59a33b 653 PostOutBI2 = 0;
maristella 19:19892c59a33b 654 }
maristella 19:19892c59a33b 655 }
maristella 19:19892c59a33b 656 PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
maristella 19:19892c59a33b 657 if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
maristella 19:19892c59a33b 658 {
maristella 19:19892c59a33b 659 fL =-fL;
maristella 19:19892c59a33b 660 // Vai indietro
maristella 19:19892c59a33b 661 AntOutBI1 = 1;
maristella 19:19892c59a33b 662 AntOutBI2 = 0;
maristella 19:19892c59a33b 663 }
maristella 19:19892c59a33b 664 else
maristella 19:19892c59a33b 665 {
maristella 19:19892c59a33b 666 if(fL >0)
maristella 19:19892c59a33b 667 {
maristella 19:19892c59a33b 668 // Vai avanti
maristella 19:19892c59a33b 669 AntOutBI1 = 0;
maristella 19:19892c59a33b 670 AntOutBI2 = 1;
maristella 19:19892c59a33b 671
maristella 19:19892c59a33b 672 }
maristella 19:19892c59a33b 673 else
maristella 19:19892c59a33b 674 {
maristella 19:19892c59a33b 675 // spegni
maristella 19:19892c59a33b 676 AntOutBI1 = 0;
maristella 19:19892c59a33b 677 AntOutBI2 = 0;
maristella 19:19892c59a33b 678 }
maristella 19:19892c59a33b 679 }
maristella 19:19892c59a33b 680 AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
maristella 19:19892c59a33b 681 } //if( (fX != fOldX) || (fY != fOldY))
maristella 19:19892c59a33b 682
maristella 19:19892c59a33b 683 //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
maristella 19:19892c59a33b 684 } //while (true) Ciclo principale
maristella 19:19892c59a33b 685
maristella 19:19892c59a33b 686 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 687 //++++++++++++++ FINE Ciclo Principale +++++++++++++++++++
maristella 19:19892c59a33b 688 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
maristella 19:19892c59a33b 689
maristella 19:19892c59a33b 690 } // main()