Amaldi / Mbed 2 deprecated Amaldi_MicRobot-Rev089

Motori e coda ok

Dependencies:   mbed

MicRobot-Rev089.cpp@21:2dd20322f02b, 2020-09-05 (annotated)

Committer:
pinofal
Date:
Sat Sep 05 16:25:42 2020 +0000
Revision:
21:2dd20322f02b
Motori e coda ok

Who changed what in which revision?

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