Amaldi
ultima versione funzionante
Dependencies: mbed
BLE-MegaPiSea_Rev2_WIP.cpp
- Committer:
- pinofal
- Date:
- 2019-07-24
- Revision:
- 3:b1e7eb9d61b7
- Parent:
- 2:fec3ed956ded
File content as of revision 3:b1e7eb9d61b7:
// 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
// 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 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 == 'A')
{
myLed = 0;
}
else
{
myLed = 1;
}
{
// 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()