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