Amaldi / Mbed 2 deprecated Amaldi_RobotSea_Rev3-4_WIP

ultima versione funzionante

Dependencies:   mbed

BLE-MegaPiSea_Rev3_WIP.cpp@6:92b0e17593e0, 2019-08-05 (annotated)

Committer:
francesco01
Date:
Mon Aug 05 09:03:43 2019 +0000
Revision:
6:92b0e17593e0
Parent:
5:b777c4f0eb63 
ultima versione

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