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?
User | Revision | Line number | New 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() |