Amaldi / Mbed 2 deprecated Amaldi_MIC_Funziona

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-Rev07_FUNZIONA.cpp@12:f1dc27a8c012, 2020-02-11 (annotated)

Committer:
pinofal
Date:
Tue Feb 11 22:20:06 2020 +0000
Revision:
12:f1dc27a8c012
Parent:
11:da53d3e94a41 
aggiornamento con funzioni di test commentate

Who changed what in which revision?

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