Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev089_1

ultima versione

Dependencies:   mbed

MicRobot-Rev089_1.cpp@22:cd1cd7259989, 2020-09-18 (annotated)

Committer:
maristella
Date:
Fri Sep 18 09:28:14 2020 +0000
Revision:
22:cd1cd7259989
ultima versione

Who changed what in which revision?

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