Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev082

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MicRobot-Rev082.cpp@14:1e80b6a5bab6, 2020-02-22 (annotated)

Committer:
pinofal
Date:
Sat Feb 22 19:09:41 2020 +0000
Revision:
14:1e80b6a5bab6
Funziona Reset

Who changed what in which revision?

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