Amaldi / Mbed 2 deprecated Amaldi_RobotSea_Rev3-4_WIP

ultima versione funzionante

Dependencies:   mbed

BLE-MegaPiSea_Rev2_WIP.cpp@3:b1e7eb9d61b7, 2019-07-24 (annotated)

Committer:
pinofal
Date:
Wed Jul 24 09:34:41 2019 +0000
Revision:
3:b1e7eb9d61b7
Parent:
2:fec3ed956ded 
resettiamo il punto di partenza

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pinofal 2:fec3ed956ded 1 // bpilotaggio carrello tramite BLE.
pinofal 2:fec3ed956ded 2 // testato su L476RG
pinofal 2:fec3ed956ded 3
pinofal 2:fec3ed956ded 4 #include "mbed.h"
pinofal 2:fec3ed956ded 5 #include<stdlib.h>
pinofal 2:fec3ed956ded 6
pinofal 2:fec3ed956ded 7 // pi greco
pinofal 2:fec3ed956ded 8 #define PI 3.14159265358979323846
pinofal 2:fec3ed956ded 9
pinofal 2:fec3ed956ded 10 // Definizione periferica USB seriale del PC
pinofal 2:fec3ed956ded 11 Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
pinofal 2:fec3ed956ded 12
pinofal 2:fec3ed956ded 13 // Definizione periferica seriale del Modulo BLE ELETT114A
pinofal 2:fec3ed956ded 14 Serial myBLE(PA_9, PA_10, 115200); //Tx, Rx, bps
pinofal 2:fec3ed956ded 15
pinofal 2:fec3ed956ded 16 // Input di Reset per il Modulo BLE ELETT114A.
pinofal 2:fec3ed956ded 17 DigitalOut BleRst(PA_8);
pinofal 2:fec3ed956ded 18
pinofal 2:fec3ed956ded 19 // User Button, LED
pinofal 2:fec3ed956ded 20 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
pinofal 2:fec3ed956ded 21 DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
pinofal 2:fec3ed956ded 22
pinofal 2:fec3ed956ded 23
pinofal 2:fec3ed956ded 24
pinofal 2:fec3ed956ded 25
pinofal 2:fec3ed956ded 26 // carattere letto dalla seriale del PC
pinofal 2:fec3ed956ded 27 char cReadChar;
pinofal 2:fec3ed956ded 28
pinofal 2:fec3ed956ded 29 // DutyCycle del segnale PWM
pinofal 2:fec3ed956ded 30 float fDutyCycle;
pinofal 2:fec3ed956ded 31
pinofal 2:fec3ed956ded 32
pinofal 2:fec3ed956ded 33 // Input/Output
pinofal 2:fec3ed956ded 34 DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
pinofal 3:b1e7eb9d61b7 35 PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore
pinofal 2:fec3ed956ded 36 //DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
pinofal 3:b1e7eb9d61b7 37 DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 2:fec3ed956ded 38 DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
pinofal 2:fec3ed956ded 39
pinofal 2:fec3ed956ded 40 //DigitalInOut OutBlades (PB_9, PIN_OUTPUT, OpenDrain, 0); // Output per il pilotaggio del Relay di azionamento Lame Rotanti
pinofal 2:fec3ed956ded 41
pinofal 2:fec3ed956ded 42
pinofal 2:fec3ed956ded 43 DigitalOut AntOutBI1 (PB_4); // Output 1 per pilotaggio input BI1 del Motore B Anteriore
pinofal 2:fec3ed956ded 44 PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore
pinofal 2:fec3ed956ded 45 //DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
pinofal 2:fec3ed956ded 46 DigitalOut AntOutBI2 (PB_3); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 2:fec3ed956ded 47 DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
pinofal 2:fec3ed956ded 48
pinofal 2:fec3ed956ded 49
pinofal 2:fec3ed956ded 50
pinofal 2:fec3ed956ded 51 /*
pinofal 2:fec3ed956ded 52 //+++DigitalIn PostInNE1 (BHO); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
pinofal 2:fec3ed956ded 53 DigitalOut PostOutBI2 (PG_15); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 2:fec3ed956ded 54 DigitalOut PostOutBI1 (PA_15); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
pinofal 2:fec3ed956ded 55 //PwmOut PostOutPWB (PH_13); // Output per pilotaggio input PWM del motore B Posteriore
pinofal 2:fec3ed956ded 56 DigitalOut PostOutPWB (PH_13); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore
pinofal 2:fec3ed956ded 57
pinofal 2:fec3ed956ded 58 DigitalOut AntOutBI2 (PB_5); // Output 2 per pilotaggio input BI2 del Motore B Posteriore
pinofal 2:fec3ed956ded 59 DigitalOut AntOutBI1 (PA_5); // Output 1 per pilotaggio input BI1 del Motore B Posteriore
pinofal 2:fec3ed956ded 60 //PwmOut AntOutPWB (PB_4); // Output per pilotaggio input PWM del motore B Posteriore
pinofal 2:fec3ed956ded 61 DigitalOut AntOutPWB (PB_4); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore
pinofal 2:fec3ed956ded 62 */
pinofal 2:fec3ed956ded 63
pinofal 2:fec3ed956ded 64
pinofal 2:fec3ed956ded 65 //carattere di comando ricevuto dal BLE
pinofal 2:fec3ed956ded 66 volatile char cCommandBLE; // cambia nella routine di interrupt
pinofal 2:fec3ed956ded 67 // memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
pinofal 2:fec3ed956ded 68 char cOldCommandBLE;
pinofal 2:fec3ed956ded 69
pinofal 2:fec3ed956ded 70 // coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
pinofal 2:fec3ed956ded 71 volatile double fTeta;
pinofal 2:fec3ed956ded 72 volatile double fRo;
pinofal 2:fec3ed956ded 73
pinofal 2:fec3ed956ded 74 // coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
pinofal 2:fec3ed956ded 75 volatile double fX, fY;
pinofal 2:fec3ed956ded 76 // memorizza ultimi valori delle coordinate del joistick
pinofal 2:fec3ed956ded 77 double fOldX, fOldY;
pinofal 2:fec3ed956ded 78
pinofal 2:fec3ed956ded 79 // variabili ausiliarie per l'algoritmo di posizionamento
pinofal 2:fec3ed956ded 80 double fV, fW;
pinofal 2:fec3ed956ded 81
pinofal 2:fec3ed956ded 82 // velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
pinofal 2:fec3ed956ded 83 double fR, fL;
pinofal 2:fec3ed956ded 84
pinofal 2:fec3ed956ded 85
pinofal 2:fec3ed956ded 86
pinofal 2:fec3ed956ded 87
pinofal 2:fec3ed956ded 88
pinofal 3:b1e7eb9d61b7 89
pinofal 2:fec3ed956ded 90 //**********************************************/
pinofal 2:fec3ed956ded 91 // IRQ associata a Rx da BLE
pinofal 2:fec3ed956ded 92 //**********************************************/
pinofal 2:fec3ed956ded 93 void BLERxInterrupt(void)
pinofal 2:fec3ed956ded 94 {
pinofal 2:fec3ed956ded 95 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 96 char cReadChar;
pinofal 2:fec3ed956ded 97
pinofal 2:fec3ed956ded 98 //indice per i cicli
pinofal 2:fec3ed956ded 99 int nIndex;
pinofal 2:fec3ed956ded 100
pinofal 2:fec3ed956ded 101 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 102 char caRxPacket[8];
pinofal 2:fec3ed956ded 103 //int nRxPacketSize;
pinofal 2:fec3ed956ded 104
pinofal 2:fec3ed956ded 105 // coordinate cartesiane della posizione joystick
pinofal 2:fec3ed956ded 106 //float fX, fY;
pinofal 2:fec3ed956ded 107 // coordinate polari della posizione joistick
pinofal 2:fec3ed956ded 108 //float fTeta;
pinofal 2:fec3ed956ded 109 //float fRo;
pinofal 2:fec3ed956ded 110
pinofal 2:fec3ed956ded 111
pinofal 2:fec3ed956ded 112
pinofal 2:fec3ed956ded 113 //pc.printf("BLE RxInterrupt: \n\r");
pinofal 2:fec3ed956ded 114
pinofal 2:fec3ed956ded 115 // ricevi caratteri su seriale, se disponibili
pinofal 2:fec3ed956ded 116 while((myBLE.readable()))
pinofal 2:fec3ed956ded 117 {
pinofal 2:fec3ed956ded 118 // acquisice stringa in input e relativa dimensione
pinofal 2:fec3ed956ded 119 cReadChar = myBLE.getc(); // Read character
pinofal 2:fec3ed956ded 120 if(cReadChar == 0x02)
pinofal 2:fec3ed956ded 121 {
pinofal 3:b1e7eb9d61b7 122 pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
pinofal 3:b1e7eb9d61b7 123
pinofal 2:fec3ed956ded 124 //-- command will be 8 bytes for joystick values
pinofal 2:fec3ed956ded 125 //-- command will be 3 bytes for button change event
pinofal 2:fec3ed956ded 126 //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
pinofal 2:fec3ed956ded 127
pinofal 2:fec3ed956ded 128
pinofal 2:fec3ed956ded 129 caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
pinofal 2:fec3ed956ded 130 cReadChar = myBLE.getc(); // legge il secondo carattere
pinofal 2:fec3ed956ded 131 if(cReadChar > 0x40)
pinofal 2:fec3ed956ded 132 {
pinofal 2:fec3ed956ded 133 // Button:
pinofal 2:fec3ed956ded 134 //-- Button events send a single character in a 3-byte packet
pinofal 2:fec3ed956ded 135 //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
pinofal 2:fec3ed956ded 136 //-- B2 uses "C" for changed to on, "D" for changed to off
pinofal 2:fec3ed956ded 137 //-- B3..B6 follow in order; valid button even characters are "A".."L"
pinofal 2:fec3ed956ded 138 caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
pinofal 2:fec3ed956ded 139 caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX
pinofal 2:fec3ed956ded 140 // passa il comando ricevuto nella variabile globale
pinofal 2:fec3ed956ded 141 cCommandBLE = caRxPacket[1];
pinofal 2:fec3ed956ded 142
pinofal 2:fec3ed956ded 143 // Diagnostica
pinofal 2:fec3ed956ded 144 /*
pinofal 2:fec3ed956ded 145 pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
pinofal 2:fec3ed956ded 146 */
pinofal 2:fec3ed956ded 147
pinofal 2:fec3ed956ded 148 }
pinofal 2:fec3ed956ded 149 else
pinofal 2:fec3ed956ded 150 {
pinofal 2:fec3ed956ded 151 // Joystick:
pinofal 2:fec3ed956ded 152 //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis
pinofal 2:fec3ed956ded 153 //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
pinofal 2:fec3ed956ded 154 //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
pinofal 2:fec3ed956ded 155 caRxPacket[1] = cReadChar; // memorizza il secondo carattere
pinofal 2:fec3ed956ded 156 for(nIndex=2; nIndex<8; nIndex++)
pinofal 2:fec3ed956ded 157 {
pinofal 2:fec3ed956ded 158 caRxPacket[nIndex] = myBLE.getc();
pinofal 2:fec3ed956ded 159 }
pinofal 2:fec3ed956ded 160 // dal messaggio estrae e visualizza le coordinate cartesiane
pinofal 2:fec3ed956ded 161 fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
pinofal 2:fec3ed956ded 162 fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
pinofal 2:fec3ed956ded 163 // converte la posizione del joistick in coordinate polari
pinofal 2:fec3ed956ded 164 fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
pinofal 2:fec3ed956ded 165 if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
pinofal 2:fec3ed956ded 166 fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
pinofal 2:fec3ed956ded 167
pinofal 2:fec3ed956ded 168 // diagnostica
pinofal 2:fec3ed956ded 169 /*
pinofal 2:fec3ed956ded 170 pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE
pinofal 2:fec3ed956ded 171 pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
pinofal 2:fec3ed956ded 172 pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
pinofal 2:fec3ed956ded 173 */
pinofal 2:fec3ed956ded 174 }
pinofal 2:fec3ed956ded 175 }
pinofal 2:fec3ed956ded 176 }
pinofal 2:fec3ed956ded 177
pinofal 2:fec3ed956ded 178 }
pinofal 2:fec3ed956ded 179
pinofal 2:fec3ed956ded 180 /**********************************************/
pinofal 2:fec3ed956ded 181 // IRQ associata a Rx da PC
pinofal 2:fec3ed956ded 182 //**********************************************/
pinofal 2:fec3ed956ded 183 void pcRxInterrupt(void)
pinofal 2:fec3ed956ded 184 {
pinofal 2:fec3ed956ded 185 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 186 char cReadChar;
pinofal 2:fec3ed956ded 187
pinofal 2:fec3ed956ded 188 // ricevi caratteri su seriale, se disponibili
pinofal 2:fec3ed956ded 189 while((pc.readable()))
pinofal 2:fec3ed956ded 190 {
pinofal 2:fec3ed956ded 191 // acquisice stringa in input e relativa dimensione
pinofal 2:fec3ed956ded 192 cReadChar = pc.getc(); // read character from PC
pinofal 2:fec3ed956ded 193 //myBLE.putc(cReadChar); // write char to BLE
pinofal 2:fec3ed956ded 194
pinofal 2:fec3ed956ded 195 //pc.printf("W>: 0x%02x\n\r",cReadChar);
pinofal 2:fec3ed956ded 196 if(cReadChar == '0') // se scrivo '0', invia questa stringa
pinofal 2:fec3ed956ded 197 {
pinofal 2:fec3ed956ded 198 //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
pinofal 2:fec3ed956ded 199 //-- button status is binary formatted string (no indicator)
pinofal 2:fec3ed956ded 200 //-- data fields sent as strings
pinofal 2:fec3ed956ded 201 //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
pinofal 2:fec3ed956ded 202 // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
pinofal 2:fec3ed956ded 203 myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
pinofal 2:fec3ed956ded 204 // diagnostica
pinofal 2:fec3ed956ded 205 /*
pinofal 2:fec3ed956ded 206 pc.printf("W>: Inviato comando a BLE\n\r");
pinofal 2:fec3ed956ded 207 */
pinofal 2:fec3ed956ded 208 }
pinofal 2:fec3ed956ded 209 }
pinofal 2:fec3ed956ded 210 }
pinofal 2:fec3ed956ded 211
pinofal 2:fec3ed956ded 212
pinofal 2:fec3ed956ded 213 /**********/
pinofal 2:fec3ed956ded 214 /* MAIN */
pinofal 2:fec3ed956ded 215 /**********/
pinofal 2:fec3ed956ded 216 int main()
pinofal 2:fec3ed956ded 217 {
pinofal 2:fec3ed956ded 218
pinofal 2:fec3ed956ded 219 // messaggio di benvenuto
pinofal 2:fec3ed956ded 220 pc.printf("\r\n******* Hallo - Exercise ********** \r\n");
pinofal 2:fec3ed956ded 221 pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n");
pinofal 2:fec3ed956ded 222
pinofal 2:fec3ed956ded 223 //Inizializza pin di output per Rotating Blades
pinofal 2:fec3ed956ded 224 /*
pinofal 2:fec3ed956ded 225 OutBlades.mode(OpenDrain);
pinofal 2:fec3ed956ded 226 OutBlades.output();
pinofal 2:fec3ed956ded 227 OutBlades.write(0);
pinofal 2:fec3ed956ded 228 nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente
pinofal 2:fec3ed956ded 229 */
pinofal 2:fec3ed956ded 230 // inizializza variabili
pinofal 2:fec3ed956ded 231 cCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 2:fec3ed956ded 232 cOldCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 2:fec3ed956ded 233 fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 234 fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 235 fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 236 fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 237
pinofal 2:fec3ed956ded 238 // inizializza il BLE
pinofal 2:fec3ed956ded 239 BleRst = 0;
pinofal 2:fec3ed956ded 240 wait_ms(100);
pinofal 2:fec3ed956ded 241 BleRst = 1;
pinofal 2:fec3ed956ded 242 cCommandBLE = '0';
pinofal 2:fec3ed956ded 243 cOldCommandBLE = '0';
pinofal 2:fec3ed956ded 244
pinofal 2:fec3ed956ded 245
pinofal 2:fec3ed956ded 246
pinofal 2:fec3ed956ded 247 // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore
pinofal 2:fec3ed956ded 248 PostOutPWB.period_us(10); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 249 PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
pinofal 2:fec3ed956ded 250 AntOutPWB.period_us(10); // periodo del PWM Anteriore
pinofal 2:fec3ed956ded 251 AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 2:fec3ed956ded 252
pinofal 2:fec3ed956ded 253
pinofal 2:fec3ed956ded 254 // Attiva la IRQ per la RX su seriale
pinofal 2:fec3ed956ded 255 myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
pinofal 2:fec3ed956ded 256 pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
pinofal 2:fec3ed956ded 257
pinofal 2:fec3ed956ded 258 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 259 //++++++++++++++ INIZIO Ciclo Principale +++++++++++++
pinofal 2:fec3ed956ded 260 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 3:b1e7eb9d61b7 261 /*
pinofal 2:fec3ed956ded 262 while(true)
pinofal 2:fec3ed956ded 263 {
pinofal 2:fec3ed956ded 264 //Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left:
pinofal 2:fec3ed956ded 265 //Invert X
pinofal 2:fec3ed956ded 266 //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
pinofal 2:fec3ed956ded 267 //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
pinofal 2:fec3ed956ded 268 //Calcola R: R = (V+W) /2
pinofal 2:fec3ed956ded 269 //Calcola L: L= (V-W)/2
pinofal 2:fec3ed956ded 270 //Scala i valori di L e R in base all'hardware.
pinofal 2:fec3ed956ded 271 //invia i valori al robot.
pinofal 2:fec3ed956ded 272
pinofal 2:fec3ed956ded 273 // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote
pinofal 2:fec3ed956ded 274 if( (fX != fOldX) || (fY != fOldY))
pinofal 2:fec3ed956ded 275 {
pinofal 2:fec3ed956ded 276 fOldX = fX;
pinofal 2:fec3ed956ded 277 fOldY = fY;
pinofal 2:fec3ed956ded 278 // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
pinofal 2:fec3ed956ded 279 fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
pinofal 2:fec3ed956ded 280 fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
pinofal 2:fec3ed956ded 281 fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
pinofal 2:fec3ed956ded 282 fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
pinofal 2:fec3ed956ded 283
pinofal 2:fec3ed956ded 284 // diagnostica
pinofal 2:fec3ed956ded 285 pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
pinofal 2:fec3ed956ded 286 pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
pinofal 2:fec3ed956ded 287 pc.printf("> Velocita' Right R = %.2f\r\n", fR);
pinofal 2:fec3ed956ded 288 pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL);
pinofal 2:fec3ed956ded 289
pinofal 2:fec3ed956ded 290 // algoritmo di movimentazione delle ruote.
pinofal 2:fec3ed956ded 291 if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
pinofal 2:fec3ed956ded 292 {
pinofal 2:fec3ed956ded 293 fR =-fR;
pinofal 2:fec3ed956ded 294 // Vai indietro
pinofal 2:fec3ed956ded 295 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 296 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 297 }
pinofal 2:fec3ed956ded 298 else
pinofal 2:fec3ed956ded 299 {
pinofal 2:fec3ed956ded 300
pinofal 2:fec3ed956ded 301 if(fR >0)
pinofal 2:fec3ed956ded 302 {
pinofal 2:fec3ed956ded 303 // Vai avanti
pinofal 2:fec3ed956ded 304 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 305 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 306 }
pinofal 2:fec3ed956ded 307 else
pinofal 2:fec3ed956ded 308 {
pinofal 2:fec3ed956ded 309 // spegni
pinofal 2:fec3ed956ded 310 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 311 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 312 }
pinofal 2:fec3ed956ded 313 }
pinofal 2:fec3ed956ded 314 PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
pinofal 2:fec3ed956ded 315 if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
pinofal 2:fec3ed956ded 316 {
pinofal 2:fec3ed956ded 317 fL =-fL;
pinofal 2:fec3ed956ded 318 // Vai indietro
pinofal 2:fec3ed956ded 319 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 320 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 321 }
pinofal 2:fec3ed956ded 322 else
pinofal 2:fec3ed956ded 323 {
pinofal 2:fec3ed956ded 324 if(fL >0)
pinofal 2:fec3ed956ded 325 {
pinofal 2:fec3ed956ded 326 // Vai avanti
pinofal 2:fec3ed956ded 327 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 328 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 329
pinofal 2:fec3ed956ded 330 }
pinofal 2:fec3ed956ded 331 else
pinofal 2:fec3ed956ded 332 {
pinofal 2:fec3ed956ded 333 // spegni
pinofal 2:fec3ed956ded 334 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 335 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 336 }
pinofal 2:fec3ed956ded 337 }
pinofal 2:fec3ed956ded 338 AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 2:fec3ed956ded 339 }
pinofal 2:fec3ed956ded 340 } //while (true) Ciclo principale
pinofal 3:b1e7eb9d61b7 341 */
pinofal 2:fec3ed956ded 342 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 343 //++++++++++++++ FINE Ciclo Principale +++++++++++++
pinofal 2:fec3ed956ded 344 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 345
pinofal 2:fec3ed956ded 346
pinofal 2:fec3ed956ded 347
pinofal 2:fec3ed956ded 348
pinofal 2:fec3ed956ded 349 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 350 //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++
pinofal 2:fec3ed956ded 351 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 3:b1e7eb9d61b7 352 //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
pinofal 3:b1e7eb9d61b7 353
pinofal 3:b1e7eb9d61b7 354 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 355 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 356 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 357 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 358
pinofal 3:b1e7eb9d61b7 359 fDutyCycle = 0.0;
pinofal 3:b1e7eb9d61b7 360
pinofal 3:b1e7eb9d61b7 361 // inizializza il pin PWM
pinofal 3:b1e7eb9d61b7 362 //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 3:b1e7eb9d61b7 363 //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 364 //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 3:b1e7eb9d61b7 365 //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 366 while(true)
pinofal 3:b1e7eb9d61b7 367 {
pinofal 3:b1e7eb9d61b7 368 if(cCommandBLE == 'A')
pinofal 3:b1e7eb9d61b7 369 {
pinofal 3:b1e7eb9d61b7 370 myLed = 0;
pinofal 3:b1e7eb9d61b7 371 }
pinofal 3:b1e7eb9d61b7 372 else
pinofal 3:b1e7eb9d61b7 373 {
pinofal 3:b1e7eb9d61b7 374 myLed = 1;
pinofal 3:b1e7eb9d61b7 375 }
pinofal 3:b1e7eb9d61b7 376 {
pinofal 3:b1e7eb9d61b7 377 // Vai avanti Anteriore
pinofal 3:b1e7eb9d61b7 378 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 379 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 380 AntOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 381 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 382 PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 383 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 384 pc.printf("Avanti Anteriore\r\n");
pinofal 3:b1e7eb9d61b7 385 wait (2);
pinofal 3:b1e7eb9d61b7 386
pinofal 3:b1e7eb9d61b7 387 // spegni
pinofal 3:b1e7eb9d61b7 388 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 389 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 390 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 391 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 392 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 393 wait (3);
pinofal 3:b1e7eb9d61b7 394
pinofal 3:b1e7eb9d61b7 395 // Vai Indietro Anteriore
pinofal 3:b1e7eb9d61b7 396 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 397 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 398 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 399 AntOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 400 PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 401 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 402 pc.printf("Indietro Anteriore \r\n");
pinofal 3:b1e7eb9d61b7 403 wait (2);
pinofal 3:b1e7eb9d61b7 404
pinofal 3:b1e7eb9d61b7 405 // spegni
pinofal 3:b1e7eb9d61b7 406 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 407 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 408 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 409 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 410 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 411 wait (3);
pinofal 3:b1e7eb9d61b7 412
pinofal 3:b1e7eb9d61b7 413 // Vai avanti Posteriore
pinofal 3:b1e7eb9d61b7 414 PostOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 415 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 416 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 417 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 418 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 419 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 420 pc.printf("Avanti Posteriore\r\n");
pinofal 3:b1e7eb9d61b7 421 wait (2);
pinofal 3:b1e7eb9d61b7 422
pinofal 3:b1e7eb9d61b7 423 // spegni
pinofal 3:b1e7eb9d61b7 424 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 425 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 426 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 427 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 428 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 429 wait (3);
pinofal 3:b1e7eb9d61b7 430
pinofal 3:b1e7eb9d61b7 431 // Vai Indietro Posteriore
pinofal 3:b1e7eb9d61b7 432 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 433 PostOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 434 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 435 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 436 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 437 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 438 pc.printf("Indietro Posteriore \r\n");
pinofal 3:b1e7eb9d61b7 439 wait (2);
pinofal 3:b1e7eb9d61b7 440
pinofal 3:b1e7eb9d61b7 441 // spegni
pinofal 3:b1e7eb9d61b7 442 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 443 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 444 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 445 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 446 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 447 wait (3);
pinofal 3:b1e7eb9d61b7 448
pinofal 3:b1e7eb9d61b7 449
pinofal 3:b1e7eb9d61b7 450
pinofal 3:b1e7eb9d61b7 451 // Vai avanti Anteriore + Posteriore
pinofal 3:b1e7eb9d61b7 452 PostOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 453 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 454 AntOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 455 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 456 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 457 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 458 pc.printf("Avanti Anteriore + Posteriore\r\n");
pinofal 3:b1e7eb9d61b7 459 wait (2);
pinofal 3:b1e7eb9d61b7 460
pinofal 3:b1e7eb9d61b7 461 // spegni
pinofal 3:b1e7eb9d61b7 462 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 463 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 464 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 465 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 466 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 467 wait (3);
pinofal 3:b1e7eb9d61b7 468
pinofal 3:b1e7eb9d61b7 469 // Vai Indietro Anteriore + Posteriore
pinofal 3:b1e7eb9d61b7 470 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 471 PostOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 472 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 473 AntOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 474 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 475 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 476 pc.printf("Indietro Anteriore + Posteriore \r\n");
pinofal 3:b1e7eb9d61b7 477 wait (2);
pinofal 3:b1e7eb9d61b7 478
pinofal 3:b1e7eb9d61b7 479 // spegni
pinofal 3:b1e7eb9d61b7 480 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 481 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 482 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 483 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 484 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 485 wait (3);
pinofal 3:b1e7eb9d61b7 486
pinofal 3:b1e7eb9d61b7 487 // Vai avanti Anteriore + Posteriore velocità ridotta
pinofal 3:b1e7eb9d61b7 488 PostOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 489 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 490 AntOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 491 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 492 PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 493 AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 494 pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
pinofal 3:b1e7eb9d61b7 495 wait (2);
pinofal 3:b1e7eb9d61b7 496
pinofal 3:b1e7eb9d61b7 497 // spegni
pinofal 3:b1e7eb9d61b7 498 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 499 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 500 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 501 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 502 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 503 wait (3);
pinofal 3:b1e7eb9d61b7 504
pinofal 3:b1e7eb9d61b7 505 // Vai Indietro Anteriore + Posteriore velocità ridotta
pinofal 3:b1e7eb9d61b7 506 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 507 PostOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 508 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 509 AntOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 510 PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 511 AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 512 pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
pinofal 3:b1e7eb9d61b7 513 wait (2);
pinofal 3:b1e7eb9d61b7 514
pinofal 3:b1e7eb9d61b7 515 // spegni
pinofal 3:b1e7eb9d61b7 516 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 517 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 518 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 519 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 520 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 521 wait (3);
pinofal 3:b1e7eb9d61b7 522
pinofal 3:b1e7eb9d61b7 523
pinofal 3:b1e7eb9d61b7 524
pinofal 3:b1e7eb9d61b7 525 // Ruota a destra
pinofal 3:b1e7eb9d61b7 526 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 527 PostOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 528 AntOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 529 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 530 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 531 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 532 pc.printf("Ruota a Destra\r\n");
pinofal 3:b1e7eb9d61b7 533 wait (2);
pinofal 3:b1e7eb9d61b7 534
pinofal 3:b1e7eb9d61b7 535 // spegni
pinofal 3:b1e7eb9d61b7 536 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 537 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 538 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 539 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 540 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 541 wait (3);
pinofal 3:b1e7eb9d61b7 542
pinofal 3:b1e7eb9d61b7 543 // Ruota a sinistra
pinofal 3:b1e7eb9d61b7 544 PostOutBI1 = 1;
pinofal 3:b1e7eb9d61b7 545 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 546 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 547 AntOutBI2 = 1;
pinofal 3:b1e7eb9d61b7 548 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 549 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 3:b1e7eb9d61b7 550 pc.printf("Ruota a Sinistra\r\n");
pinofal 3:b1e7eb9d61b7 551 wait (2);
pinofal 3:b1e7eb9d61b7 552
pinofal 3:b1e7eb9d61b7 553 // spegni
pinofal 3:b1e7eb9d61b7 554 PostOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 555 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 556 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 557 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 558 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 559 wait (3);
pinofal 3:b1e7eb9d61b7 560
pinofal 3:b1e7eb9d61b7 561 }
pinofal 3:b1e7eb9d61b7 562 } // while(true) Test motore con PWM
pinofal 3:b1e7eb9d61b7 563
pinofal 3:b1e7eb9d61b7 564 //++++++++++++ FINE Test Motore con PWM ++++++++++++
pinofal 2:fec3ed956ded 565
pinofal 2:fec3ed956ded 566 //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++
pinofal 3:b1e7eb9d61b7 567 /*
pinofal 2:fec3ed956ded 568 // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
pinofal 2:fec3ed956ded 569 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 570 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 571 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 572 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 573
pinofal 2:fec3ed956ded 574 fDutyCycle = 0.0;
pinofal 2:fec3ed956ded 575
pinofal 2:fec3ed956ded 576 // inizializza il pin PWM
pinofal 2:fec3ed956ded 577 PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 578 PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 579 AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 2:fec3ed956ded 580 AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 581 while(true)
pinofal 2:fec3ed956ded 582 {
pinofal 2:fec3ed956ded 583 //Parsing del comando ricevuto solo se cambia il comando.
pinofal 2:fec3ed956ded 584 if(cCommandBLE != cOldCommandBLE)
pinofal 2:fec3ed956ded 585 {
pinofal 2:fec3ed956ded 586 // ricorda il vecchio comando
pinofal 2:fec3ed956ded 587 cOldCommandBLE = cCommandBLE;
pinofal 2:fec3ed956ded 588 //cambia velocità del PWM
pinofal 2:fec3ed956ded 589 fDutyCycle = double(cOldCommandBLE - 0x41)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
pinofal 2:fec3ed956ded 590 PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore
pinofal 2:fec3ed956ded 591 AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore
pinofal 2:fec3ed956ded 592 pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle);
pinofal 2:fec3ed956ded 593 //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM
pinofal 2:fec3ed956ded 594 //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0));
pinofal 2:fec3ed956ded 595
pinofal 2:fec3ed956ded 596 //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione
pinofal 2:fec3ed956ded 597 switch (cOldCommandBLE) // usa cOldCommandBLE.
pinofal 2:fec3ed956ded 598 {
pinofal 2:fec3ed956ded 599 case 'A':
pinofal 2:fec3ed956ded 600 {
pinofal 2:fec3ed956ded 601 // se ricevi il comando 'A'
pinofal 2:fec3ed956ded 602 // Vai avanti
pinofal 2:fec3ed956ded 603 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 604 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 605 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 606 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 607 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 608 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 609 pc.printf("Avanti\r\n\r\n");
pinofal 2:fec3ed956ded 610 wait (2);
pinofal 2:fec3ed956ded 611
pinofal 2:fec3ed956ded 612 // spegni
pinofal 2:fec3ed956ded 613 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 614 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 615 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 616 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 617 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 618 wait (1);
pinofal 2:fec3ed956ded 619 }break;
pinofal 2:fec3ed956ded 620 case 'B':
pinofal 2:fec3ed956ded 621 {
pinofal 2:fec3ed956ded 622 // se ricevi il comando 'B'
pinofal 2:fec3ed956ded 623 // Vai Indietro
pinofal 2:fec3ed956ded 624 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 625 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 626 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 627 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 628 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 629 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 630 pc.printf("Indietro\r\n\r\n");
pinofal 2:fec3ed956ded 631 wait (2);
pinofal 2:fec3ed956ded 632
pinofal 2:fec3ed956ded 633 // spegni
pinofal 2:fec3ed956ded 634 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 635 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 636 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 637 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 638 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 639 wait (1);
pinofal 2:fec3ed956ded 640 } break;
pinofal 2:fec3ed956ded 641
pinofal 2:fec3ed956ded 642 case 'C':
pinofal 2:fec3ed956ded 643 {
pinofal 2:fec3ed956ded 644 // se ricevi il comando 'C'
pinofal 2:fec3ed956ded 645 // Ruota a destra
pinofal 2:fec3ed956ded 646 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 647 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 648 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 649 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 650 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 651 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 652 pc.printf("Destra\r\n\r\n");
pinofal 2:fec3ed956ded 653 wait (3.0);
pinofal 2:fec3ed956ded 654
pinofal 2:fec3ed956ded 655 // spegni
pinofal 2:fec3ed956ded 656 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 657 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 658 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 659 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 660 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 661 wait (1);
pinofal 2:fec3ed956ded 662 } break;
pinofal 2:fec3ed956ded 663 case 'D':
pinofal 2:fec3ed956ded 664 {
pinofal 2:fec3ed956ded 665 // se ricevi il comando 'D'
pinofal 2:fec3ed956ded 666 // Ruota a sinistra
pinofal 2:fec3ed956ded 667 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 668 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 669 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 670 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 671 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 672 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 673 pc.printf("Sinistra\r\n\r\n");
pinofal 2:fec3ed956ded 674 wait (3.0);
pinofal 2:fec3ed956ded 675
pinofal 2:fec3ed956ded 676 // spegni
pinofal 2:fec3ed956ded 677 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 678 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 679 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 680 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 681 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 682 wait (1);
pinofal 2:fec3ed956ded 683 } break;
pinofal 2:fec3ed956ded 684 case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti
pinofal 2:fec3ed956ded 685 {
pinofal 2:fec3ed956ded 686 // se ricevi il comando 'L' Cambia direzione di movimento delle ruote
pinofal 2:fec3ed956ded 687 PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0
pinofal 2:fec3ed956ded 688 AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0
pinofal 2:fec3ed956ded 689
pinofal 2:fec3ed956ded 690 // inverti direzione di movimento
pinofal 2:fec3ed956ded 691 PostOutBI1 = !PostOutBI1;
pinofal 2:fec3ed956ded 692 PostOutBI2 = !PostOutBI2;
pinofal 2:fec3ed956ded 693 AntOutBI1 = !AntOutBI1;
pinofal 2:fec3ed956ded 694 AntOutBI2 = !AntOutBI2;
pinofal 2:fec3ed956ded 695 // ricomincia a bassa velocità
pinofal 2:fec3ed956ded 696 PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore
pinofal 2:fec3ed956ded 697 AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore
pinofal 2:fec3ed956ded 698 } break;
pinofal 2:fec3ed956ded 699 default:
pinofal 2:fec3ed956ded 700 {
pinofal 2:fec3ed956ded 701 // se ricevi comandi diversi, spegni i motori
pinofal 2:fec3ed956ded 702 // spegni
pinofal 2:fec3ed956ded 703 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 704 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 705 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 706 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 707 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 708 wait (1);
pinofal 2:fec3ed956ded 709 } break;
pinofal 2:fec3ed956ded 710 }
pinofal 2:fec3ed956ded 711 }
pinofal 2:fec3ed956ded 712 } // while(true) Test motore con PWM pilotato da BLE
pinofal 3:b1e7eb9d61b7 713 */
pinofal 2:fec3ed956ded 714 //++++++++++++ FINE Test PWM tramite BLE ++++++++++++
pinofal 2:fec3ed956ded 715
pinofal 3:b1e7eb9d61b7 716
pinofal 3:b1e7eb9d61b7 717 /*
pinofal 3:b1e7eb9d61b7 718 //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++
pinofal 2:fec3ed956ded 719 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 720 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 721 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 722 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 723
pinofal 2:fec3ed956ded 724 fDutyCycle = 0.0;
pinofal 2:fec3ed956ded 725
pinofal 2:fec3ed956ded 726 // inizializza il pin PWM
pinofal 2:fec3ed956ded 727 PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 728 PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 729 //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 3:b1e7eb9d61b7 730 //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 731 while(true)
pinofal 2:fec3ed956ded 732 {
pinofal 3:b1e7eb9d61b7 733 //+++if(cCommandBLE == 'A')
pinofal 2:fec3ed956ded 734 {
pinofal 2:fec3ed956ded 735 // Vai avanti
pinofal 2:fec3ed956ded 736 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 737 PostOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 738 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 739 AntOutBI2 = 0;
pinofal 3:b1e7eb9d61b7 740 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 741 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 742 pc.printf("Avanti\r\n\r\n");
pinofal 2:fec3ed956ded 743 wait (2);
pinofal 2:fec3ed956ded 744
pinofal 2:fec3ed956ded 745 // spegni
pinofal 2:fec3ed956ded 746 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 747 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 748 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 749 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 750 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 751 wait (3);
pinofal 2:fec3ed956ded 752
pinofal 2:fec3ed956ded 753 // Vai Indietro
pinofal 2:fec3ed956ded 754 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 755 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 756 AntOutBI1 = 0;
pinofal 3:b1e7eb9d61b7 757 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 758 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 3:b1e7eb9d61b7 759 AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 760 pc.printf("Indietro\r\n\r\n");
pinofal 2:fec3ed956ded 761 wait (2);
pinofal 2:fec3ed956ded 762
pinofal 2:fec3ed956ded 763 // spegni
pinofal 2:fec3ed956ded 764 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 765 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 766 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 767 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 768 pc.printf("Fermo\r\n\r\n");
pinofal 3:b1e7eb9d61b7 769 wait (3);
pinofal 2:fec3ed956ded 770
pinofal 2:fec3ed956ded 771
pinofal 2:fec3ed956ded 772
pinofal 2:fec3ed956ded 773 }
pinofal 3:b1e7eb9d61b7 774 } // while(true) Test motore con PWM
pinofal 2:fec3ed956ded 775 */
pinofal 3:b1e7eb9d61b7 776 //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++
pinofal 3:b1e7eb9d61b7 777
pinofal 3:b1e7eb9d61b7 778
pinofal 2:fec3ed956ded 779
pinofal 2:fec3ed956ded 780 //++++++++++++ INIZIO Test Motore ++++++++++++
pinofal 2:fec3ed956ded 781 /*
pinofal 2:fec3ed956ded 782 while(1)
pinofal 2:fec3ed956ded 783 {
pinofal 2:fec3ed956ded 784 //if(myButton == 0)
pinofal 2:fec3ed956ded 785 {
pinofal 2:fec3ed956ded 786 // CW
pinofal 2:fec3ed956ded 787 PostOutPWB = 1;
pinofal 2:fec3ed956ded 788 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 789 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 790 AntOutPWB = 1;
pinofal 2:fec3ed956ded 791 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 792 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 793 pc.printf("CW\r\n\r\n");
pinofal 2:fec3ed956ded 794 wait (2);
pinofal 2:fec3ed956ded 795
pinofal 2:fec3ed956ded 796
pinofal 2:fec3ed956ded 797 // spegni
pinofal 2:fec3ed956ded 798 PostOutPWB = 1;
pinofal 2:fec3ed956ded 799 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 800 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 801 AntOutPWB = 1;
pinofal 2:fec3ed956ded 802 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 803 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 804 wait (1);
pinofal 2:fec3ed956ded 805
pinofal 2:fec3ed956ded 806
pinofal 2:fec3ed956ded 807 // CCW
pinofal 2:fec3ed956ded 808 PostOutPWB = 1;
pinofal 2:fec3ed956ded 809 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 810 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 811 AntOutPWB = 1;
pinofal 2:fec3ed956ded 812 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 813 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 814 wait (2);
pinofal 2:fec3ed956ded 815 pc.printf("CCW\r\n\r\n");
pinofal 2:fec3ed956ded 816
pinofal 2:fec3ed956ded 817 // spegni
pinofal 2:fec3ed956ded 818 PostOutPWB = 1;
pinofal 2:fec3ed956ded 819 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 820 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 821 AntOutPWB = 1;
pinofal 2:fec3ed956ded 822 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 823 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 824
pinofal 2:fec3ed956ded 825 wait (1);
pinofal 2:fec3ed956ded 826
pinofal 2:fec3ed956ded 827 }
pinofal 2:fec3ed956ded 828 } // while(true) test motore
pinofal 2:fec3ed956ded 829 */
pinofal 2:fec3ed956ded 830 //++++++++++++ FINE Test Motore ++++++++++++
pinofal 2:fec3ed956ded 831
pinofal 2:fec3ed956ded 832
pinofal 2:fec3ed956ded 833 //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++
pinofal 2:fec3ed956ded 834 /*
pinofal 2:fec3ed956ded 835 // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
pinofal 2:fec3ed956ded 836 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 837 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 838 fDutyCycle = 0.5;
pinofal 2:fec3ed956ded 839
pinofal 2:fec3ed956ded 840 // inizializza il pin PWM
pinofal 2:fec3ed956ded 841 PostOutPWB.period_ms(1000); // periodo del PWM
pinofal 2:fec3ed956ded 842 PostOutPWB.write(fDutyCycle); // duty cycle del PWM
pinofal 2:fec3ed956ded 843 while(true)
pinofal 2:fec3ed956ded 844 {
pinofal 2:fec3ed956ded 845 // verifica se è arrivato un carattere dalla seriale del PC
pinofal 2:fec3ed956ded 846 if(pc.readable())
pinofal 2:fec3ed956ded 847 {
pinofal 2:fec3ed956ded 848 cReadChar = pc.getc(); // Read hyperterminal
pinofal 2:fec3ed956ded 849 fDutyCycle = float(cReadChar - 0x30)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
pinofal 2:fec3ed956ded 850 pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle);
pinofal 2:fec3ed956ded 851 //PostOutPWB.period_ms(1000); // periodo del PWM
pinofal 2:fec3ed956ded 852 //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM
pinofal 2:fec3ed956ded 853
pinofal 2:fec3ed956ded 854 // Inverti direzione del moto
pinofal 2:fec3ed956ded 855 //PostOutBI1 != PostOutBI1;
pinofal 2:fec3ed956ded 856 //PostOutBI2 != PostOutBI2;
pinofal 2:fec3ed956ded 857
pinofal 2:fec3ed956ded 858 PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM
pinofal 2:fec3ed956ded 859
pinofal 2:fec3ed956ded 860 }
pinofal 2:fec3ed956ded 861 } // while(true) test PWM tramite seriale del PC
pinofal 2:fec3ed956ded 862 */
pinofal 2:fec3ed956ded 863 //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++
pinofal 2:fec3ed956ded 864
pinofal 2:fec3ed956ded 865 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 866 //++++++++++++++ FINE Ciclo test +++++++++++++++++++
pinofal 2:fec3ed956ded 867 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 868
pinofal 2:fec3ed956ded 869 } // main()