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