ultima versione funzionante
Dependencies: mbed
Diff: BLE-MegaPiSea_Rev3_WIP.cpp
- Revision:
- 4:19d1ac0f3ec2
- Parent:
- 3:b1e7eb9d61b7
- Child:
- 5:b777c4f0eb63
diff -r b1e7eb9d61b7 -r 19d1ac0f3ec2 BLE-MegaPiSea_Rev3_WIP.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BLE-MegaPiSea_Rev3_WIP.cpp Fri Jul 26 07:19:10 2019 +0000 @@ -0,0 +1,911 @@ +// bpilotaggio carrello tramite BLE. +// testato su L476RG + +#include "mbed.h" +#include<stdlib.h> + +// pi greco +#define PI 3.14159265358979323846 + +// Definizione periferica USB seriale del PC +Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 + +// Definizione periferica seriale del Modulo BLE ELETT114A +Serial myBLE(PA_9, PA_10, 115200); //Tx, Rx, bps + +// Input di Reset per il Modulo BLE ELETT114A. +DigitalOut BleRst(PA_8); + +// User Button, LED +DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 +DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5 + +DigitalOut Light(PA_0); + + +// carattere letto dalla seriale del PC +char cReadChar; + +// DutyCycle del segnale PWM +float fDutyCycle; + + +// Input/Output +DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore +PwmOut PostOutPWB (PB_6); // Output per pilotaggio input PWM del motore B Posteriore +//DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore +DigitalOut PostOutBI2 (PA_7); // Output 2 per pilotaggio input BI2 del Motore B Posteriore +DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore + +//DigitalInOut OutBlades (PB_9, PIN_OUTPUT, OpenDrain, 0); // Output per il pilotaggio del Relay di azionamento Lame Rotanti + + +DigitalOut AntOutBI1 (PB_4); // Output 1 per pilotaggio input BI1 del Motore B Anteriore +PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore +//DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore +DigitalOut AntOutBI2 (PB_3); // Output 2 per pilotaggio input BI2 del Motore B Posteriore +DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore + + + +/* +//+++DigitalIn PostInNE1 (BHO); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore +DigitalOut PostOutBI2 (PG_15); // Output 2 per pilotaggio input BI2 del Motore B Posteriore +DigitalOut PostOutBI1 (PA_15); // Output 1 per pilotaggio input BI1 del Motore B Posteriore +//PwmOut PostOutPWB (PH_13); // Output per pilotaggio input PWM del motore B Posteriore +DigitalOut PostOutPWB (PH_13); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore + +DigitalOut AntOutBI2 (PB_5); // Output 2 per pilotaggio input BI2 del Motore B Posteriore +DigitalOut AntOutBI1 (PA_5); // Output 1 per pilotaggio input BI1 del Motore B Posteriore +//PwmOut AntOutPWB (PB_4); // Output per pilotaggio input PWM del motore B Posteriore +DigitalOut AntOutPWB (PB_4); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore +*/ + + +//carattere di comando ricevuto dal BLE +volatile char cCommandBLE; // cambia nella routine di interrupt +// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente +char cOldCommandBLE; + +// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt +volatile double fTeta; +volatile double fRo; + +// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt +volatile double fX, fY; +// memorizza ultimi valori delle coordinate del joistick +double fOldX, fOldY; + +// variabili ausiliarie per l'algoritmo di posizionamento +double fV, fW; + +// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore +double fR, fL; + + + + + + +//**********************************************/ +// IRQ associata a Rx da BLE +//**********************************************/ +void BLERxInterrupt(void) +{ + // array per la ricezione dei messaggi da seriale + char cReadChar; + + //indice per i cicli + int nIndex; + + // array per la ricezione dei messaggi da seriale + char caRxPacket[8]; + //int nRxPacketSize; + + // coordinate cartesiane della posizione joystick + //float fX, fY; + // coordinate polari della posizione joistick + //float fTeta; + //float fRo; + + + + //pc.printf("BLE RxInterrupt: \n\r"); + + // ricevi caratteri su seriale, se disponibili + while((myBLE.readable())) + { + // acquisice stringa in input e relativa dimensione + cReadChar = myBLE.getc(); // Read character + if(cReadChar == 0x02) + { + pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6 + + //-- command will be 8 bytes for joystick values + //-- command will be 3 bytes for button change event + //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03) + + + caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX + cReadChar = myBLE.getc(); // legge il secondo carattere + if(cReadChar > 0x40) + { + // Button: + //-- Button events send a single character in a 3-byte packet + //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX> + //-- B2 uses "C" for changed to on, "D" for changed to off + //-- B3..B6 follow in order; valid button even characters are "A".."L" + caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP + caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX + // passa il comando ricevuto nella variabile globale + cCommandBLE = caRxPacket[1]; + + // Diagnostica + /* + 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 + */ + + } + else + { + // Joystick: + //-- 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 + //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator + //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX> + caRxPacket[1] = cReadChar; // memorizza il secondo carattere + for(nIndex=2; nIndex<8; nIndex++) + { + caRxPacket[nIndex] = myBLE.getc(); + } + // dal messaggio estrae e visualizza le coordinate cartesiane + fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200); + fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200); + // converte la posizione del joistick in coordinate polari + fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo + if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360° + fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100 + + // diagnostica + /* + 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 + pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane + pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari + */ + } + } + } + +} + +/**********************************************/ +// IRQ associata a Rx da PC +//**********************************************/ +void pcRxInterrupt(void) +{ + // array per la ricezione dei messaggi da seriale + char cReadChar; + + // ricevi caratteri su seriale, se disponibili + while((pc.readable())) + { + // acquisice stringa in input e relativa dimensione + cReadChar = pc.getc(); // read character from PC + //myBLE.putc(cReadChar); // write char to BLE + + //pc.printf("W>: 0x%02x\n\r",cReadChar); + if(cReadChar == '0') // se scrivo '0', invia questa stringa + { + //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX> + //-- button status is binary formatted string (no indicator) + //-- data fields sent as strings + //-- send empty string to unused field (not sure if short response packet is allowed without additional testing) + // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX; + 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); + // diagnostica + /* + pc.printf("W>: Inviato comando a BLE\n\r"); + */ + } + } +} + + +/**********/ +/* MAIN */ +/**********/ +int main() +{ + + // messaggio di benvenuto + pc.printf("\r\n******* Hallo - Exercise ********** \r\n"); + pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n"); + + //Inizializza pin di output per Rotating Blades + /* + OutBlades.mode(OpenDrain); + OutBlades.output(); + OutBlades.write(0); + nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente + */ + // inizializza variabili + cCommandBLE = 0; // inizialmente nessun comando da BLE + cOldCommandBLE = 0; // inizialmente nessun comando da BLE + fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) + fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) + fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) + fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) + + // inizializza il BLE + BleRst = 0; + wait_ms(100); + BleRst = 1; + cCommandBLE = '0'; + cOldCommandBLE = '0'; + + + + // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore + PostOutPWB.period_us(10); // periodo del PWM Posteriore + PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore) + AntOutPWB.period_us(10); // periodo del PWM Anteriore + AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore) + + + // Attiva la IRQ per la RX su seriale + myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE + pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC + + //++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ INIZIO Ciclo Principale +++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++ + /* + while(true) + { + //Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left: + //Invert X + //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y + //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X + //Calcola R: R = (V+W) /2 + //Calcola L: L= (V-W)/2 + //Scala i valori di L e R in base all'hardware. + //invia i valori al robot. + + // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote + if( (fX != fOldX) || (fY != fOldY)) + { + fOldX = fX; + fOldY = fY; + // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL) + fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio + fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio + fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100) + fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100) + + // diagnostica + pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); + pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); + pc.printf("> Velocita' Right R = %.2f\r\n", fR); + pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); + + // algoritmo di movimentazione delle ruote. + if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore + { + fR =-fR; + // Vai indietro + PostOutBI1 = 1; + PostOutBI2 = 0; + } + else + { + + if(fR >0) + { + // Vai avanti + PostOutBI1 = 0; + PostOutBI2 = 1; + } + else + { + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + } + } + PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore) + if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore + { + fL =-fL; + // Vai indietro + AntOutBI1 = 1; + AntOutBI2 = 0; + } + else + { + if(fL >0) + { + // Vai avanti + AntOutBI1 = 0; + AntOutBI2 = 1; + + } + else + { + // spegni + AntOutBI1 = 0; + AntOutBI2 = 0; + } + } + AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore) + } + } //while (true) Ciclo principale + */ + //++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ FINE Ciclo Principale +++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++ + + + + + //++++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++ + /* + //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++ + while(true) + { + if(cCommandBLE != cOldCommandBLE) + { + + switch (cCommandBLE) + { + case 'A': + { + myLed = 1; + }; break; + case 'B': + { + myLed = 0; + }; break; + case 'C': + { + Light = 1; + }; break; + case 'D': + { + Light = 0; + }; break; + default: break; + } + // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica + cOldCommandBLE = cCommandBLE; + } + + } // while(true) Test comandi da BLE + + //++++++++++++ FINE ricezione comandi BLE ++++++++++++ + */ + //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++ + + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + + fDutyCycle = 0.0; + + // inizializza il pin PWM + //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore + //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore + //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore + //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore + while(true) + { + if(cCommandBLE != cOldCommandBLE) + { + + if(cCommandBLE == 'A') + { + myLed = 1; + + + } + + else + { + myLed = 0; + } + pc.printf("Comando = %c \r\n", cCommandBLE); + cOldCommandBLE = cCommandBLE; + } + { + // Vai avanti Anteriore + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(0.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Avanti Anteriore\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai Indietro Anteriore + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(0.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Indietro Anteriore \r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai avanti Posteriore + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(0.0); // duty cycle del PWM Anteriore + pc.printf("Avanti Posteriore\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai Indietro Posteriore + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 0; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(0.0); // duty cycle del PWM Anteriore + pc.printf("Indietro Posteriore \r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai avanti Anteriore + Posteriore + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Avanti Anteriore + Posteriore\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai Indietro Anteriore + Posteriore + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Indietro Anteriore + Posteriore \r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai avanti Anteriore + Posteriore velocità ridotta + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(0.5); // duty cycle del PWM Posteriore + AntOutPWB.write(0.5); // duty cycle del PWM Anteriore + pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai Indietro Anteriore + Posteriore velocità ridotta + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(0.5); // duty cycle del PWM Posteriore + AntOutPWB.write(0.5); // duty cycle del PWM Anteriore + pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + + + // Ruota a destra + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Ruota a Destra\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Ruota a sinistra + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Ruota a Sinistra\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + } + } // while(true) Test motore con PWM + + //++++++++++++ FINE Test Motore con PWM ++++++++++++ + + //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++ + /* + // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 1; + AntOutBI2 = 0; + + fDutyCycle = 0.0; + + // inizializza il pin PWM + PostOutPWB.period_us(100); // periodo del PWM Posteriore + PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore + AntOutPWB.period_us(100); // periodo del PWM Anteriore + AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore + while(true) + { + //Parsing del comando ricevuto solo se cambia il comando. + if(cCommandBLE != cOldCommandBLE) + { + // ricorda il vecchio comando + cOldCommandBLE = cCommandBLE; + //cambia velocità del PWM + 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 + PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore + AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore + pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle); + //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM + //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0)); + + //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione + switch (cOldCommandBLE) // usa cOldCommandBLE. + { + case 'A': + { + // se ricevi il comando 'A' + // Vai avanti + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Avanti\r\n\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (1); + }break; + case 'B': + { + // se ricevi il comando 'B' + // Vai Indietro + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Indietro\r\n\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (1); + } break; + + case 'C': + { + // se ricevi il comando 'C' + // Ruota a destra + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 1; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Destra\r\n\r\n"); + wait (3.0); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (1); + } break; + case 'D': + { + // se ricevi il comando 'D' + // Ruota a sinistra + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 1; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(1.0); // duty cycle del PWM Anteriore + pc.printf("Sinistra\r\n\r\n"); + wait (3.0); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (1); + } break; + case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti + { + // se ricevi il comando 'L' Cambia direzione di movimento delle ruote + PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0 + AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0 + + // inverti direzione di movimento + PostOutBI1 = !PostOutBI1; + PostOutBI2 = !PostOutBI2; + AntOutBI1 = !AntOutBI1; + AntOutBI2 = !AntOutBI2; + // ricomincia a bassa velocità + PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore + AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore + } break; + default: + { + // se ricevi comandi diversi, spegni i motori + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (1); + } break; + } + } + } // while(true) Test motore con PWM pilotato da BLE + */ + //++++++++++++ FINE Test PWM tramite BLE ++++++++++++ + + + /* + //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++ + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + + fDutyCycle = 0.0; + + // inizializza il pin PWM + PostOutPWB.period_us(100); // periodo del PWM Posteriore + PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore + //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore + //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore + while(true) + { + //+++if(cCommandBLE == 'A') + { + // Vai avanti + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(0.0); // duty cycle del PWM Anteriore + pc.printf("Avanti\r\n\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + // Vai Indietro + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutBI1 = 0; + AntOutBI2 = 0; + PostOutPWB.write(1.0); // duty cycle del PWM Posteriore + AntOutPWB.write(0.0); // duty cycle del PWM Anteriore + pc.printf("Indietro\r\n\r\n"); + wait (2); + + // spegni + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutBI1 = 0; + AntOutBI2 = 0; + pc.printf("Fermo\r\n\r\n"); + wait (3); + + + + } + } // while(true) Test motore con PWM + */ + //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++ + + + + //++++++++++++ INIZIO Test Motore ++++++++++++ + /* + while(1) + { + //if(myButton == 0) + { + // CW + PostOutPWB = 1; + PostOutBI1 = 1; + PostOutBI2 = 0; + AntOutPWB = 1; + AntOutBI1 = 0; + AntOutBI2 = 1; + pc.printf("CW\r\n\r\n"); + wait (2); + + + // spegni + PostOutPWB = 1; + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutPWB = 1; + AntOutBI1 = 0; + AntOutBI2 = 0; + wait (1); + + + // CCW + PostOutPWB = 1; + PostOutBI1 = 0; + PostOutBI2 = 1; + AntOutPWB = 1; + AntOutBI1 = 1; + AntOutBI2 = 0; + wait (2); + pc.printf("CCW\r\n\r\n"); + + // spegni + PostOutPWB = 1; + PostOutBI1 = 0; + PostOutBI2 = 0; + AntOutPWB = 1; + AntOutBI1 = 0; + AntOutBI2 = 0; + + wait (1); + + } + } // while(true) test motore + */ + //++++++++++++ FINE Test Motore ++++++++++++ + + + //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++ + /* + // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' + PostOutBI1 = 1; + PostOutBI2 = 0; + fDutyCycle = 0.5; + + // inizializza il pin PWM + PostOutPWB.period_ms(1000); // periodo del PWM + PostOutPWB.write(fDutyCycle); // duty cycle del PWM + while(true) + { + // verifica se è arrivato un carattere dalla seriale del PC + if(pc.readable()) + { + cReadChar = pc.getc(); // Read hyperterminal + 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 + pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle); + //PostOutPWB.period_ms(1000); // periodo del PWM + //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM + + // Inverti direzione del moto + //PostOutBI1 != PostOutBI1; + //PostOutBI2 != PostOutBI2; + + PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM + + } + } // while(true) test PWM tramite seriale del PC + */ + //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++ + + //++++++++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++ FINE Ciclo test +++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++ + +} // main()