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