Amaldi / Mbed 2 deprecated Amaldi_RobotSea_Rev3-4_WIP

ultima versione funzionante

Dependencies:   mbed

BLE-MegaPiSea_Rev2_WIP.cpp@2:fec3ed956ded, 2019-06-27 (annotated)

Committer:
pinofal
Date:
Thu Jun 27 09:08:17 2019 +0000
Revision:
2:fec3ed956ded
Child:
3:b1e7eb9d61b7
Robot Funzionante

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 2:fec3ed956ded 35 PwmOut PostOutPWB (PA_7); // 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 2:fec3ed956ded 37 DigitalOut PostOutBI2 (PB_6); // 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 2:fec3ed956ded 89 //**********************************************/
pinofal 2:fec3ed956ded 90 // IRQ associata a Rx da BLE
pinofal 2:fec3ed956ded 91 //**********************************************/
pinofal 2:fec3ed956ded 92 void BLERxInterrupt(void)
pinofal 2:fec3ed956ded 93 {
pinofal 2:fec3ed956ded 94 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 95 char cReadChar;
pinofal 2:fec3ed956ded 96
pinofal 2:fec3ed956ded 97 //indice per i cicli
pinofal 2:fec3ed956ded 98 int nIndex;
pinofal 2:fec3ed956ded 99
pinofal 2:fec3ed956ded 100 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 101 char caRxPacket[8];
pinofal 2:fec3ed956ded 102 //int nRxPacketSize;
pinofal 2:fec3ed956ded 103
pinofal 2:fec3ed956ded 104 // coordinate cartesiane della posizione joystick
pinofal 2:fec3ed956ded 105 //float fX, fY;
pinofal 2:fec3ed956ded 106 // coordinate polari della posizione joistick
pinofal 2:fec3ed956ded 107 //float fTeta;
pinofal 2:fec3ed956ded 108 //float fRo;
pinofal 2:fec3ed956ded 109
pinofal 2:fec3ed956ded 110
pinofal 2:fec3ed956ded 111
pinofal 2:fec3ed956ded 112 //pc.printf("BLE RxInterrupt: \n\r");
pinofal 2:fec3ed956ded 113
pinofal 2:fec3ed956ded 114 // ricevi caratteri su seriale, se disponibili
pinofal 2:fec3ed956ded 115 while((myBLE.readable()))
pinofal 2:fec3ed956ded 116 {
pinofal 2:fec3ed956ded 117 // acquisice stringa in input e relativa dimensione
pinofal 2:fec3ed956ded 118 cReadChar = myBLE.getc(); // Read character
pinofal 2:fec3ed956ded 119 if(cReadChar == 0x02)
pinofal 2:fec3ed956ded 120 {
pinofal 2:fec3ed956ded 121 //-- command will be 8 bytes for joystick values
pinofal 2:fec3ed956ded 122 //-- command will be 3 bytes for button change event
pinofal 2:fec3ed956ded 123 //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
pinofal 2:fec3ed956ded 124
pinofal 2:fec3ed956ded 125
pinofal 2:fec3ed956ded 126 caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
pinofal 2:fec3ed956ded 127 cReadChar = myBLE.getc(); // legge il secondo carattere
pinofal 2:fec3ed956ded 128 if(cReadChar > 0x40)
pinofal 2:fec3ed956ded 129 {
pinofal 2:fec3ed956ded 130 // Button:
pinofal 2:fec3ed956ded 131 //-- Button events send a single character in a 3-byte packet
pinofal 2:fec3ed956ded 132 //-- 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 133 //-- B2 uses "C" for changed to on, "D" for changed to off
pinofal 2:fec3ed956ded 134 //-- B3..B6 follow in order; valid button even characters are "A".."L"
pinofal 2:fec3ed956ded 135 caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
pinofal 2:fec3ed956ded 136 caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX
pinofal 2:fec3ed956ded 137 // passa il comando ricevuto nella variabile globale
pinofal 2:fec3ed956ded 138 cCommandBLE = caRxPacket[1];
pinofal 2:fec3ed956ded 139
pinofal 2:fec3ed956ded 140 // Diagnostica
pinofal 2:fec3ed956ded 141 /*
pinofal 2:fec3ed956ded 142 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 143 */
pinofal 2:fec3ed956ded 144
pinofal 2:fec3ed956ded 145 }
pinofal 2:fec3ed956ded 146 else
pinofal 2:fec3ed956ded 147 {
pinofal 2:fec3ed956ded 148 // Joystick:
pinofal 2:fec3ed956ded 149 //-- 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 150 //-- 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 151 //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
pinofal 2:fec3ed956ded 152 caRxPacket[1] = cReadChar; // memorizza il secondo carattere
pinofal 2:fec3ed956ded 153 for(nIndex=2; nIndex<8; nIndex++)
pinofal 2:fec3ed956ded 154 {
pinofal 2:fec3ed956ded 155 caRxPacket[nIndex] = myBLE.getc();
pinofal 2:fec3ed956ded 156 }
pinofal 2:fec3ed956ded 157 // dal messaggio estrae e visualizza le coordinate cartesiane
pinofal 2:fec3ed956ded 158 fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
pinofal 2:fec3ed956ded 159 fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
pinofal 2:fec3ed956ded 160 // converte la posizione del joistick in coordinate polari
pinofal 2:fec3ed956ded 161 fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
pinofal 2:fec3ed956ded 162 if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
pinofal 2:fec3ed956ded 163 fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
pinofal 2:fec3ed956ded 164
pinofal 2:fec3ed956ded 165 // diagnostica
pinofal 2:fec3ed956ded 166 /*
pinofal 2:fec3ed956ded 167 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 168 pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
pinofal 2:fec3ed956ded 169 pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
pinofal 2:fec3ed956ded 170 */
pinofal 2:fec3ed956ded 171 }
pinofal 2:fec3ed956ded 172 }
pinofal 2:fec3ed956ded 173 }
pinofal 2:fec3ed956ded 174
pinofal 2:fec3ed956ded 175 }
pinofal 2:fec3ed956ded 176
pinofal 2:fec3ed956ded 177 /**********************************************/
pinofal 2:fec3ed956ded 178 // IRQ associata a Rx da PC
pinofal 2:fec3ed956ded 179 //**********************************************/
pinofal 2:fec3ed956ded 180 void pcRxInterrupt(void)
pinofal 2:fec3ed956ded 181 {
pinofal 2:fec3ed956ded 182 // array per la ricezione dei messaggi da seriale
pinofal 2:fec3ed956ded 183 char cReadChar;
pinofal 2:fec3ed956ded 184
pinofal 2:fec3ed956ded 185 // ricevi caratteri su seriale, se disponibili
pinofal 2:fec3ed956ded 186 while((pc.readable()))
pinofal 2:fec3ed956ded 187 {
pinofal 2:fec3ed956ded 188 // acquisice stringa in input e relativa dimensione
pinofal 2:fec3ed956ded 189 cReadChar = pc.getc(); // read character from PC
pinofal 2:fec3ed956ded 190 //myBLE.putc(cReadChar); // write char to BLE
pinofal 2:fec3ed956ded 191
pinofal 2:fec3ed956ded 192 //pc.printf("W>: 0x%02x\n\r",cReadChar);
pinofal 2:fec3ed956ded 193 if(cReadChar == '0') // se scrivo '0', invia questa stringa
pinofal 2:fec3ed956ded 194 {
pinofal 2:fec3ed956ded 195 //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 196 //-- button status is binary formatted string (no indicator)
pinofal 2:fec3ed956ded 197 //-- data fields sent as strings
pinofal 2:fec3ed956ded 198 //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
pinofal 2:fec3ed956ded 199 // 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 200 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 201 // diagnostica
pinofal 2:fec3ed956ded 202 /*
pinofal 2:fec3ed956ded 203 pc.printf("W>: Inviato comando a BLE\n\r");
pinofal 2:fec3ed956ded 204 */
pinofal 2:fec3ed956ded 205 }
pinofal 2:fec3ed956ded 206 }
pinofal 2:fec3ed956ded 207 }
pinofal 2:fec3ed956ded 208
pinofal 2:fec3ed956ded 209
pinofal 2:fec3ed956ded 210 /**********/
pinofal 2:fec3ed956ded 211 /* MAIN */
pinofal 2:fec3ed956ded 212 /**********/
pinofal 2:fec3ed956ded 213 int main()
pinofal 2:fec3ed956ded 214 {
pinofal 2:fec3ed956ded 215
pinofal 2:fec3ed956ded 216 // messaggio di benvenuto
pinofal 2:fec3ed956ded 217 pc.printf("\r\n******* Hallo - Exercise ********** \r\n");
pinofal 2:fec3ed956ded 218 pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n");
pinofal 2:fec3ed956ded 219
pinofal 2:fec3ed956ded 220 //Inizializza pin di output per Rotating Blades
pinofal 2:fec3ed956ded 221 /*
pinofal 2:fec3ed956ded 222 OutBlades.mode(OpenDrain);
pinofal 2:fec3ed956ded 223 OutBlades.output();
pinofal 2:fec3ed956ded 224 OutBlades.write(0);
pinofal 2:fec3ed956ded 225 nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente
pinofal 2:fec3ed956ded 226 */
pinofal 2:fec3ed956ded 227 // inizializza variabili
pinofal 2:fec3ed956ded 228 cCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 2:fec3ed956ded 229 cOldCommandBLE = 0; // inizialmente nessun comando da BLE
pinofal 2:fec3ed956ded 230 fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 231 fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 232 fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 233 fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
pinofal 2:fec3ed956ded 234
pinofal 2:fec3ed956ded 235 // inizializza il BLE
pinofal 2:fec3ed956ded 236 BleRst = 0;
pinofal 2:fec3ed956ded 237 wait_ms(100);
pinofal 2:fec3ed956ded 238 BleRst = 1;
pinofal 2:fec3ed956ded 239 cCommandBLE = '0';
pinofal 2:fec3ed956ded 240 cOldCommandBLE = '0';
pinofal 2:fec3ed956ded 241
pinofal 2:fec3ed956ded 242
pinofal 2:fec3ed956ded 243
pinofal 2:fec3ed956ded 244 // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore
pinofal 2:fec3ed956ded 245 PostOutPWB.period_us(10); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 246 PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
pinofal 2:fec3ed956ded 247 AntOutPWB.period_us(10); // periodo del PWM Anteriore
pinofal 2:fec3ed956ded 248 AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 2:fec3ed956ded 249
pinofal 2:fec3ed956ded 250
pinofal 2:fec3ed956ded 251 // Attiva la IRQ per la RX su seriale
pinofal 2:fec3ed956ded 252 myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
pinofal 2:fec3ed956ded 253 pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
pinofal 2:fec3ed956ded 254
pinofal 2:fec3ed956ded 255 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 256 //++++++++++++++ INIZIO Ciclo Principale +++++++++++++
pinofal 2:fec3ed956ded 257 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 258
pinofal 2:fec3ed956ded 259 while(true)
pinofal 2:fec3ed956ded 260 {
pinofal 2:fec3ed956ded 261 //Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left:
pinofal 2:fec3ed956ded 262 //Invert X
pinofal 2:fec3ed956ded 263 //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
pinofal 2:fec3ed956ded 264 //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
pinofal 2:fec3ed956ded 265 //Calcola R: R = (V+W) /2
pinofal 2:fec3ed956ded 266 //Calcola L: L= (V-W)/2
pinofal 2:fec3ed956ded 267 //Scala i valori di L e R in base all'hardware.
pinofal 2:fec3ed956ded 268 //invia i valori al robot.
pinofal 2:fec3ed956ded 269
pinofal 2:fec3ed956ded 270 // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote
pinofal 2:fec3ed956ded 271 if( (fX != fOldX) || (fY != fOldY))
pinofal 2:fec3ed956ded 272 {
pinofal 2:fec3ed956ded 273 fOldX = fX;
pinofal 2:fec3ed956ded 274 fOldY = fY;
pinofal 2:fec3ed956ded 275 // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
pinofal 2:fec3ed956ded 276 fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
pinofal 2:fec3ed956ded 277 fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
pinofal 2:fec3ed956ded 278 fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
pinofal 2:fec3ed956ded 279 fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
pinofal 2:fec3ed956ded 280
pinofal 2:fec3ed956ded 281 // diagnostica
pinofal 2:fec3ed956ded 282 pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
pinofal 2:fec3ed956ded 283 pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
pinofal 2:fec3ed956ded 284 pc.printf("> Velocita' Right R = %.2f\r\n", fR);
pinofal 2:fec3ed956ded 285 pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL);
pinofal 2:fec3ed956ded 286
pinofal 2:fec3ed956ded 287 // algoritmo di movimentazione delle ruote.
pinofal 2:fec3ed956ded 288 if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
pinofal 2:fec3ed956ded 289 {
pinofal 2:fec3ed956ded 290 fR =-fR;
pinofal 2:fec3ed956ded 291 // Vai indietro
pinofal 2:fec3ed956ded 292 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 293 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 294 }
pinofal 2:fec3ed956ded 295 else
pinofal 2:fec3ed956ded 296 {
pinofal 2:fec3ed956ded 297
pinofal 2:fec3ed956ded 298 if(fR >0)
pinofal 2:fec3ed956ded 299 {
pinofal 2:fec3ed956ded 300 // Vai avanti
pinofal 2:fec3ed956ded 301 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 302 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 303 }
pinofal 2:fec3ed956ded 304 else
pinofal 2:fec3ed956ded 305 {
pinofal 2:fec3ed956ded 306 // spegni
pinofal 2:fec3ed956ded 307 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 308 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 309 }
pinofal 2:fec3ed956ded 310 }
pinofal 2:fec3ed956ded 311 PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
pinofal 2:fec3ed956ded 312 if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
pinofal 2:fec3ed956ded 313 {
pinofal 2:fec3ed956ded 314 fL =-fL;
pinofal 2:fec3ed956ded 315 // Vai indietro
pinofal 2:fec3ed956ded 316 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 317 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 318 }
pinofal 2:fec3ed956ded 319 else
pinofal 2:fec3ed956ded 320 {
pinofal 2:fec3ed956ded 321 if(fL >0)
pinofal 2:fec3ed956ded 322 {
pinofal 2:fec3ed956ded 323 // Vai avanti
pinofal 2:fec3ed956ded 324 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 325 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 326
pinofal 2:fec3ed956ded 327 }
pinofal 2:fec3ed956ded 328 else
pinofal 2:fec3ed956ded 329 {
pinofal 2:fec3ed956ded 330 // spegni
pinofal 2:fec3ed956ded 331 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 332 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 333 }
pinofal 2:fec3ed956ded 334 }
pinofal 2:fec3ed956ded 335 AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
pinofal 2:fec3ed956ded 336 }
pinofal 2:fec3ed956ded 337 } //while (true) Ciclo principale
pinofal 2:fec3ed956ded 338
pinofal 2:fec3ed956ded 339 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 340 //++++++++++++++ FINE Ciclo Principale +++++++++++++
pinofal 2:fec3ed956ded 341 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 342
pinofal 2:fec3ed956ded 343
pinofal 2:fec3ed956ded 344
pinofal 2:fec3ed956ded 345
pinofal 2:fec3ed956ded 346 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 347 //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++
pinofal 2:fec3ed956ded 348 //++++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 349
pinofal 2:fec3ed956ded 350 //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++
pinofal 2:fec3ed956ded 351
pinofal 2:fec3ed956ded 352 // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
pinofal 2:fec3ed956ded 353 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 354 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 355 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 356 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 357
pinofal 2:fec3ed956ded 358 fDutyCycle = 0.0;
pinofal 2:fec3ed956ded 359
pinofal 2:fec3ed956ded 360 // inizializza il pin PWM
pinofal 2:fec3ed956ded 361 PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 362 PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 363 AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 2:fec3ed956ded 364 AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 365 while(true)
pinofal 2:fec3ed956ded 366 {
pinofal 2:fec3ed956ded 367 //Parsing del comando ricevuto solo se cambia il comando.
pinofal 2:fec3ed956ded 368 if(cCommandBLE != cOldCommandBLE)
pinofal 2:fec3ed956ded 369 {
pinofal 2:fec3ed956ded 370 // ricorda il vecchio comando
pinofal 2:fec3ed956ded 371 cOldCommandBLE = cCommandBLE;
pinofal 2:fec3ed956ded 372 //cambia velocità del PWM
pinofal 2:fec3ed956ded 373 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 374 PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore
pinofal 2:fec3ed956ded 375 AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore
pinofal 2:fec3ed956ded 376 pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle);
pinofal 2:fec3ed956ded 377 //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM
pinofal 2:fec3ed956ded 378 //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0));
pinofal 2:fec3ed956ded 379
pinofal 2:fec3ed956ded 380 //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione
pinofal 2:fec3ed956ded 381 switch (cOldCommandBLE) // usa cOldCommandBLE.
pinofal 2:fec3ed956ded 382 {
pinofal 2:fec3ed956ded 383 case 'A':
pinofal 2:fec3ed956ded 384 {
pinofal 2:fec3ed956ded 385 // se ricevi il comando 'A'
pinofal 2:fec3ed956ded 386 // Vai avanti
pinofal 2:fec3ed956ded 387 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 388 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 389 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 390 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 391 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 392 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 393 pc.printf("Avanti\r\n\r\n");
pinofal 2:fec3ed956ded 394 wait (2);
pinofal 2:fec3ed956ded 395
pinofal 2:fec3ed956ded 396 // spegni
pinofal 2:fec3ed956ded 397 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 398 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 399 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 400 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 401 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 402 wait (1);
pinofal 2:fec3ed956ded 403 }break;
pinofal 2:fec3ed956ded 404 case 'B':
pinofal 2:fec3ed956ded 405 {
pinofal 2:fec3ed956ded 406 // se ricevi il comando 'B'
pinofal 2:fec3ed956ded 407 // Vai Indietro
pinofal 2:fec3ed956ded 408 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 409 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 410 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 411 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 412 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 413 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 414 pc.printf("Indietro\r\n\r\n");
pinofal 2:fec3ed956ded 415 wait (2);
pinofal 2:fec3ed956ded 416
pinofal 2:fec3ed956ded 417 // spegni
pinofal 2:fec3ed956ded 418 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 419 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 420 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 421 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 422 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 423 wait (1);
pinofal 2:fec3ed956ded 424 } break;
pinofal 2:fec3ed956ded 425
pinofal 2:fec3ed956ded 426 case 'C':
pinofal 2:fec3ed956ded 427 {
pinofal 2:fec3ed956ded 428 // se ricevi il comando 'C'
pinofal 2:fec3ed956ded 429 // Ruota a destra
pinofal 2:fec3ed956ded 430 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 431 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 432 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 433 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 434 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 435 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 436 pc.printf("Destra\r\n\r\n");
pinofal 2:fec3ed956ded 437 wait (3.0);
pinofal 2:fec3ed956ded 438
pinofal 2:fec3ed956ded 439 // spegni
pinofal 2:fec3ed956ded 440 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 441 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 442 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 443 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 444 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 445 wait (1);
pinofal 2:fec3ed956ded 446 } break;
pinofal 2:fec3ed956ded 447 case 'D':
pinofal 2:fec3ed956ded 448 {
pinofal 2:fec3ed956ded 449 // se ricevi il comando 'D'
pinofal 2:fec3ed956ded 450 // Ruota a sinistra
pinofal 2:fec3ed956ded 451 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 452 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 453 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 454 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 455 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 456 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 457 pc.printf("Sinistra\r\n\r\n");
pinofal 2:fec3ed956ded 458 wait (3.0);
pinofal 2:fec3ed956ded 459
pinofal 2:fec3ed956ded 460 // spegni
pinofal 2:fec3ed956ded 461 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 462 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 463 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 464 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 465 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 466 wait (1);
pinofal 2:fec3ed956ded 467 } break;
pinofal 2:fec3ed956ded 468 case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti
pinofal 2:fec3ed956ded 469 {
pinofal 2:fec3ed956ded 470 // se ricevi il comando 'L' Cambia direzione di movimento delle ruote
pinofal 2:fec3ed956ded 471 PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0
pinofal 2:fec3ed956ded 472 AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0
pinofal 2:fec3ed956ded 473
pinofal 2:fec3ed956ded 474 // inverti direzione di movimento
pinofal 2:fec3ed956ded 475 PostOutBI1 = !PostOutBI1;
pinofal 2:fec3ed956ded 476 PostOutBI2 = !PostOutBI2;
pinofal 2:fec3ed956ded 477 AntOutBI1 = !AntOutBI1;
pinofal 2:fec3ed956ded 478 AntOutBI2 = !AntOutBI2;
pinofal 2:fec3ed956ded 479 // ricomincia a bassa velocità
pinofal 2:fec3ed956ded 480 PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore
pinofal 2:fec3ed956ded 481 AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore
pinofal 2:fec3ed956ded 482 } break;
pinofal 2:fec3ed956ded 483 default:
pinofal 2:fec3ed956ded 484 {
pinofal 2:fec3ed956ded 485 // se ricevi comandi diversi, spegni i motori
pinofal 2:fec3ed956ded 486 // spegni
pinofal 2:fec3ed956ded 487 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 488 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 489 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 490 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 491 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 492 wait (1);
pinofal 2:fec3ed956ded 493 } break;
pinofal 2:fec3ed956ded 494 }
pinofal 2:fec3ed956ded 495 }
pinofal 2:fec3ed956ded 496 } // while(true) Test motore con PWM pilotato da BLE
pinofal 2:fec3ed956ded 497
pinofal 2:fec3ed956ded 498 //++++++++++++ FINE Test PWM tramite BLE ++++++++++++
pinofal 2:fec3ed956ded 499
pinofal 2:fec3ed956ded 500 //++++++++++++ INIZIO Test Motore con PWM ++++++++++++
pinofal 2:fec3ed956ded 501 /*
pinofal 2:fec3ed956ded 502 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 503 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 504 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 505 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 506
pinofal 2:fec3ed956ded 507 fDutyCycle = 0.0;
pinofal 2:fec3ed956ded 508
pinofal 2:fec3ed956ded 509 // inizializza il pin PWM
pinofal 2:fec3ed956ded 510 PostOutPWB.period_us(100); // periodo del PWM Posteriore
pinofal 2:fec3ed956ded 511 PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 512 AntOutPWB.period_us(100); // periodo del PWM Anteriore
pinofal 2:fec3ed956ded 513 AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 514 while(true)
pinofal 2:fec3ed956ded 515 {
pinofal 2:fec3ed956ded 516 if(cCommandBLE == 'A')
pinofal 2:fec3ed956ded 517 {
pinofal 2:fec3ed956ded 518 // Vai avanti
pinofal 2:fec3ed956ded 519 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 520 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 521 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 522 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 523 PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 524 AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 525 pc.printf("Avanti\r\n\r\n");
pinofal 2:fec3ed956ded 526 wait (2);
pinofal 2:fec3ed956ded 527
pinofal 2:fec3ed956ded 528 // spegni
pinofal 2:fec3ed956ded 529 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 530 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 531 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 532 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 533 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 534 wait (1);
pinofal 2:fec3ed956ded 535
pinofal 2:fec3ed956ded 536 // Vai Indietro
pinofal 2:fec3ed956ded 537 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 538 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 539 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 540 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 541 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 542 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 543 pc.printf("Indietro\r\n\r\n");
pinofal 2:fec3ed956ded 544 wait (2);
pinofal 2:fec3ed956ded 545
pinofal 2:fec3ed956ded 546 // spegni
pinofal 2:fec3ed956ded 547 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 548 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 549 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 550 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 551 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 552 wait (1);
pinofal 2:fec3ed956ded 553
pinofal 2:fec3ed956ded 554 // Ruota a destra
pinofal 2:fec3ed956ded 555 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 556 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 557 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 558 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 559 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 560 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 561 pc.printf("Destra\r\n\r\n");
pinofal 2:fec3ed956ded 562 wait (2);
pinofal 2:fec3ed956ded 563
pinofal 2:fec3ed956ded 564 // spegni
pinofal 2:fec3ed956ded 565 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 566 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 567 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 568 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 569 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 570 wait (1);
pinofal 2:fec3ed956ded 571
pinofal 2:fec3ed956ded 572 // Ruota a sinistra
pinofal 2:fec3ed956ded 573 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 574 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 575 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 576 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 577 PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
pinofal 2:fec3ed956ded 578 AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
pinofal 2:fec3ed956ded 579 pc.printf("Sinistra\r\n\r\n");
pinofal 2:fec3ed956ded 580 wait (2);
pinofal 2:fec3ed956ded 581
pinofal 2:fec3ed956ded 582 // spegni
pinofal 2:fec3ed956ded 583 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 584 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 585 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 586 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 587 pc.printf("Fermo\r\n\r\n");
pinofal 2:fec3ed956ded 588 wait (1);
pinofal 2:fec3ed956ded 589
pinofal 2:fec3ed956ded 590 }
pinofal 2:fec3ed956ded 591 } // while(true) Test motore con PWM
pinofal 2:fec3ed956ded 592 */
pinofal 2:fec3ed956ded 593 //++++++++++++ FINE Test Motore con PWM ++++++++++++
pinofal 2:fec3ed956ded 594
pinofal 2:fec3ed956ded 595 //++++++++++++ INIZIO Test Motore ++++++++++++
pinofal 2:fec3ed956ded 596 /*
pinofal 2:fec3ed956ded 597 while(1)
pinofal 2:fec3ed956ded 598 {
pinofal 2:fec3ed956ded 599 //if(myButton == 0)
pinofal 2:fec3ed956ded 600 {
pinofal 2:fec3ed956ded 601 // CW
pinofal 2:fec3ed956ded 602 PostOutPWB = 1;
pinofal 2:fec3ed956ded 603 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 604 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 605 AntOutPWB = 1;
pinofal 2:fec3ed956ded 606 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 607 AntOutBI2 = 1;
pinofal 2:fec3ed956ded 608 pc.printf("CW\r\n\r\n");
pinofal 2:fec3ed956ded 609 wait (2);
pinofal 2:fec3ed956ded 610
pinofal 2:fec3ed956ded 611
pinofal 2:fec3ed956ded 612 // spegni
pinofal 2:fec3ed956ded 613 PostOutPWB = 1;
pinofal 2:fec3ed956ded 614 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 615 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 616 AntOutPWB = 1;
pinofal 2:fec3ed956ded 617 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 618 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 619 wait (1);
pinofal 2:fec3ed956ded 620
pinofal 2:fec3ed956ded 621
pinofal 2:fec3ed956ded 622 // CCW
pinofal 2:fec3ed956ded 623 PostOutPWB = 1;
pinofal 2:fec3ed956ded 624 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 625 PostOutBI2 = 1;
pinofal 2:fec3ed956ded 626 AntOutPWB = 1;
pinofal 2:fec3ed956ded 627 AntOutBI1 = 1;
pinofal 2:fec3ed956ded 628 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 629 wait (2);
pinofal 2:fec3ed956ded 630 pc.printf("CCW\r\n\r\n");
pinofal 2:fec3ed956ded 631
pinofal 2:fec3ed956ded 632 // spegni
pinofal 2:fec3ed956ded 633 PostOutPWB = 1;
pinofal 2:fec3ed956ded 634 PostOutBI1 = 0;
pinofal 2:fec3ed956ded 635 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 636 AntOutPWB = 1;
pinofal 2:fec3ed956ded 637 AntOutBI1 = 0;
pinofal 2:fec3ed956ded 638 AntOutBI2 = 0;
pinofal 2:fec3ed956ded 639
pinofal 2:fec3ed956ded 640 wait (1);
pinofal 2:fec3ed956ded 641
pinofal 2:fec3ed956ded 642 }
pinofal 2:fec3ed956ded 643 } // while(true) test motore
pinofal 2:fec3ed956ded 644 */
pinofal 2:fec3ed956ded 645 //++++++++++++ FINE Test Motore ++++++++++++
pinofal 2:fec3ed956ded 646
pinofal 2:fec3ed956ded 647
pinofal 2:fec3ed956ded 648 //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++
pinofal 2:fec3ed956ded 649 /*
pinofal 2:fec3ed956ded 650 // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
pinofal 2:fec3ed956ded 651 PostOutBI1 = 1;
pinofal 2:fec3ed956ded 652 PostOutBI2 = 0;
pinofal 2:fec3ed956ded 653 fDutyCycle = 0.5;
pinofal 2:fec3ed956ded 654
pinofal 2:fec3ed956ded 655 // inizializza il pin PWM
pinofal 2:fec3ed956ded 656 PostOutPWB.period_ms(1000); // periodo del PWM
pinofal 2:fec3ed956ded 657 PostOutPWB.write(fDutyCycle); // duty cycle del PWM
pinofal 2:fec3ed956ded 658 while(true)
pinofal 2:fec3ed956ded 659 {
pinofal 2:fec3ed956ded 660 // verifica se è arrivato un carattere dalla seriale del PC
pinofal 2:fec3ed956ded 661 if(pc.readable())
pinofal 2:fec3ed956ded 662 {
pinofal 2:fec3ed956ded 663 cReadChar = pc.getc(); // Read hyperterminal
pinofal 2:fec3ed956ded 664 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 665 pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle);
pinofal 2:fec3ed956ded 666 //PostOutPWB.period_ms(1000); // periodo del PWM
pinofal 2:fec3ed956ded 667 //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM
pinofal 2:fec3ed956ded 668
pinofal 2:fec3ed956ded 669 // Inverti direzione del moto
pinofal 2:fec3ed956ded 670 //PostOutBI1 != PostOutBI1;
pinofal 2:fec3ed956ded 671 //PostOutBI2 != PostOutBI2;
pinofal 2:fec3ed956ded 672
pinofal 2:fec3ed956ded 673 PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM
pinofal 2:fec3ed956ded 674
pinofal 2:fec3ed956ded 675 }
pinofal 2:fec3ed956ded 676 } // while(true) test PWM tramite seriale del PC
pinofal 2:fec3ed956ded 677 */
pinofal 2:fec3ed956ded 678 //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++
pinofal 2:fec3ed956ded 679
pinofal 2:fec3ed956ded 680 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 681 //++++++++++++++ FINE Ciclo test +++++++++++++++++++
pinofal 2:fec3ed956ded 682 //++++++++++++++++++++++++++++++++++++++++++++++++++
pinofal 2:fec3ed956ded 683
pinofal 2:fec3ed956ded 684 } // main()