Amaldi / Mbed 2 deprecated Amaldi_Campanella_Rev01

Problemi

Dependencies:   mbed

MicRobot-Rev085.cpp@18:4b1d35d5f4fd, 2020-07-08 (annotated)

Committer:
pinofal
Date:
Wed Jul 08 19:55:47 2020 +0000
Revision:
18:4b1d35d5f4fd
Child:
19:733cea5788a4
Potrebbe funzionare con la nuova APP

Who changed what in which revision?

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