Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev085

funziona tutto e i motori non entrano in loop

Dependencies:   mbed

MicRobot-Rev084.cpp@17:485beedd4a55, 2020-05-20 (annotated)

Committer:
maristella
Date:
Wed May 20 18:32:35 2020 +0000
Revision:
17:485beedd4a55
Parent:
16:2581bc608372 
fuonziona tutto e non entra in loop

Who changed what in which revision?

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