Amaldi
Amaldi Robot Ver. 1.0
Dependencies: mbed X-NUCLEO-IHM05A1
Revision 13:24b2e46b0fcf, committed 2019-02-14
- Comitter:
- pinofal
- Date:
- Thu Feb 14 16:33:38 2019 +0000
- Parent:
- 12:00ce5d30b82c
- Commit message:
- Robot Rev 1.0
Changed in this revision
| RobotFinale.cpp | Show diff for this revision Revisions of this file |
| RobotFinale_Rev1.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/RobotFinale.cpp Thu Nov 22 00:42:32 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,426 +0,0 @@
-/* mbed specific header files. */
-#include "mbed.h"
-
-/* Component specific header files. */
-#include "L6208.h"
-
-// numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
-#define SAMPLESINENUM 45 // consigliabile avere multipli di 45
-
-// numero di campioni acquisiti su cui effettuare la media di luminosità
-#define NUMLIGHTSAMPLE 100
-
-// parametri dell'onda coseno da generare
-#define PI (3.141592653589793238462)
-#define AMPLITUDE 32767 //(1.0) // x * 3.3V
-#define PHASE (PI/2) // 2*pi è un periodo
-#define OFFSET 32767 //(0x7FFF)
-
-// Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
-#define SOGLIALUCI (1.5)
-
-// Definizione Parametri per il motore
-#ifdef TARGET_NUCLEO_F334R8
-#define VREFA_PWM_PIN D11
-#define VREFB_PWM_PIN D9
-#elif TARGET_NUCLEO_F302R8
-#define VREFA_PWM_PIN D11
-#define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
-#else
-#define VREFA_PWM_PIN D3
-#define VREFB_PWM_PIN D9
-#endif
-
-// definizioni funzioni di default su scheda
-Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
-DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
-DigitalOut led2(LED2);// LED verde sulla scheda
-
-// pin A2 di output per la forma d'onda analogica dedicata al suono
-AnalogOut OutWave(PA_4);
-
-// pin A1 di input per la forma d'onda analogica dedicata alla luminosità
-AnalogIn InWaveLight(PA_1);
-
-// Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
-DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0);
-// Timer per il calcolo dei tempi del sensore di prossimità
-Timer myTimer;
-
-// tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
-int nTimerStart, nTimer, nTimerStop;
-
-// distanza in cm dell'ostacolo
-double fDistance;
-
-// Buffer contenente la sinusoide da porre in output.
-unsigned short usaSine[SAMPLESINENUM];
-
-// prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
-void CalculateSinewave(void);
-
-
-// Periodo di generazione campioni in output DeltaT = T/NumSample
-double fDeltaT;
-// amplificazione per il dato da spedire sull'ADC
-double fAmp;
-
-// frequenza segnale audio da generare
-double fFreq;
-
-// periodo della sinusoide audio da generare
-double fPeriod;
-
-// indice per i cicli
-int nIndex;
-
-// numero di campioni di onda sonora già generati
-int nSampleCount;
-
-
-// valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
-double fAvgLight;
-
-// valore numerico, di tensione e di luce letto dall'ADC
-volatile unsigned short usReadADC;
-volatile float fReadVoltage;
-
-// valore di luminosità letto dall'ADC
-volatile float fLight;
-
-// Output Digitali usati per i LED
-DigitalOut LedWAD (PC_2);
-DigitalOut LedWAS (PC_3);
-DigitalOut LedWPD (PC_10);
-DigitalOut LedWPS (PA_0) ;
-/*XXXXXXXX ATTENZIONE NON UTILIZZABILE COME LED PERCHE' UTILIZZATO DA IHM05A1 XXXXXXX
-DigitalOut LedYAD (PC_13);
-*/
-DigitalOut LedYAS (PC_14);
-DigitalOut LedRPD (PC_12);
-/*XXXXXXXX ATTENZIONE NON UTILIZZABILE COME LED PERCHE' UTILIZZATO DA SENSORE DI LUMINOSITA' XXXXXXX
-DigitalOut LedRPS (PA_1) ;
-*/
-// Input/Output Digitali usati per interfaccia RPI
-DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
-DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
-DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
-DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
-DigitalIn InFutureUse2RPI (PC_15); // usi futuri 1 di comunicazione = RPI GPIO12
-
-/****************************************************************************/
-/* Initialization parameters of the motor connected to the expansion board. */
-/****************************************************************************/
-l6208_init_t init =
-{
-
- 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 20, //Acceleration current torque in % (from 0 to 100)
- 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 30, //Deceleration current torque in % (from 0 to 100)
- 1500, //Running speed in step/s or (1/16)th step/s for microstep modes
- 50, //Running current torque in % (from 0 to 100)
- 20, //Holding current torque in % (from 0 to 100)
- STEP_MODE_1_16, //Step mode via enum motorStepMode_t
- FAST_DECAY, //Decay mode via enum motorDecayMode_t
- 0, //Dwelling time in ms
- FALSE, //Automatic HIZ STOP
- 100000 //VREFA and VREFB PWM frequency (Hz)
-};
-
-/* Motor Control Component. */
-L6208 *motor;
-
-
-//****************************
-// Create the sinewave buffer
-//****************************
-void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
-{
- // variabile contenente l'angolo in radianti
- double fRads;
- // indici per i cicli
- int nIndex;
- // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
- double fDeltaF;
- // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
- double fAngle;
-
- fDeltaF = 360.0/SAMPLESINENUM;
- for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++)
- {
- fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
- fRads = (PI * fAngle)/180.0; // Convert degree in radian
- //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
- usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
- }
-}
-
-/********************************************************/
-/* Funzione avviata all'inizio come saluto e Benvenuto */
-/********************************************************/
-void WelcomeMessage()
-{
-}
-
-/********************************************************************/
-/* brief This is an example of user handler for the flag interrupt.*/
-/* param None */
-/* retval None */
-/* note If needed, implement it, and then attach and enable it: */
-/* + motor->attach_flag_irq(&my_flag_irq_handler); */
-/* + motor->enable_flag_irq(); */
-/* To disable it: */
-/* + motor->DisbleFlagIRQ(); */
-/*********************************************************************/
-void my_flag_irq_handler(void)
-{
- pc.printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
- motor->disable();
- pc.printf(" Motor disabled.\r\n\n");
-}
-
-/***************************************************************
- * brief This is an example of error handler.
- * param[in] error Number of the error
- * retval None
- * note If needed, implement it, and then attach it:
- * + motor->attach_error_handler(&my_error_handler);
- **************************************************************/
-void my_error_handler(uint16_t error)
-{
- /* Printing to the console. */
- pc.printf("Error %d detected\r\n\n", error);
-
- /* Infinite loop */
- while (true)
- {}
-}
-
-/********/
-/* Main */
-/********/
-int main()
-{
- // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
- pc.baud(921600); //921600 bps
- //pc.baud(9600); //256000 bps
- pc.printf("*** Test Motor ***\n\r");
-
- /* Initializing Motor Control Component. */
- motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
- if (motor->init(&init) != COMPONENT_OK)
- {
- exit(EXIT_FAILURE);
- }
-
- /* Attaching and enabling an interrupt handler. */
- motor->attach_flag_irq(&my_flag_irq_handler);
- motor->enable_flag_irq();
-
- /* Attaching an error handler */
- motor->attach_error_handler(&my_error_handler);
-
- /* Printing to the console. */
- pc.printf("Motor Control Application Example for 1 Motor\r\n");
-
- //----- run the motor BACKWARD
- pc.printf("--> Running the motor backward.\r\n");
- motor->run(StepperMotor::BWD);
-
-
- //----- Soft stop required while running
- pc.printf("--> Soft stop requested.\r\n");
- motor->soft_stop();
-
-
- //----- Change step mode to full step mode
- motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
- pc.printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->get_step_mode());
-
- /* Get current position of device and print to the console */
- pc.printf(" Position: %d.\r\n", motor->get_position());
-
- /* Set speed, acceleration and deceleration to scale with normal mode */
- motor->set_max_speed(init.maxSpeedSps>>4);
- motor->set_acceleration(motor->get_acceleration()>>4);
- motor->set_deceleration(motor->get_deceleration()>>4);
- /* Print parameters to the console */
- pc.printf(" Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
- pc.printf(" Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
- pc.printf(" Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
- pc.printf(" Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
-
- //----- move of 200 steps in the FW direction
- pc.printf("--> Moving forward 200 steps.\r\n");
- motor->move(StepperMotor::FWD, 200);
-
- //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
- fFreq = 440.0; // frequenza in Hz del tono da generare
- fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
- fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
- CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
- //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
-
- /*
- while(true)
- {
-
- }
- */
-
- //++++ INIZIO Ciclo Principale ++++
- while (true)
- {
- //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
- if(InMotorSwitchRPI==1)
- {
- // Request device to go position -3200
- motor->go_to(150);
- // Waiting while the motor is active.
- motor->wait_while_active();
- }
- else
- {
- // Request device to go position -3200
- motor->go_to(-150);
- // Waiting while the motor is active.
- motor->wait_while_active();
- }
- //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
- //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
- if(InLightSwitchRPI ==1)
- {
- // accendi i LED di abbellimento
- led2=1;
- }
- else
- {
- // spegni i LED di abbellimento
- led2=0;
- }
- //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
- //++++++++++++ INIZIO Misura della Luminosità e accensione LED ++++++++++++++
- // inizializza il valore medio della Luminosità
- fAvgLight=0.0;
- for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
- {
- // acquisisce dato da ADC
- usReadADC = InWaveLight.read_u16();
- fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
- //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
- fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
- fAvgLight+=fLight;
- }
- // calcola valore medio su NUMSAMPLE acquisizioni
- fAvgLight/= NUMLIGHTSAMPLE;
- // Accendi LED Bianchi se illuminazione è sottosoglia
- if(fAvgLight < SOGLIALUCI)
- {
- led2 = 1;
- }
- else
- {
- led2 = 0;
- }
-
- // invia il dato al PC
- //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
- //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
-
-
- //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
- //inizializza misura di distanza
- fDistance=0.0;
- // Fissa come Output il pin myProx
- myProx.output();
- // Poni 'L' sul Pin e mantienilo per qualche microsecondo
- myProx.write(0);
- wait_us(5);
- // Poni 'H' sul Pin e mantienilo per qualche microsecondo
- myProx.write(1);
- wait_us(10);
- // Poni 'L' sul Pin e mantienilo per qualche microsecondo
- myProx.write(0);
- // Attendi assestamento e Fissa come Input il pin myProx
- wait_us(5);
- myProx.input();
-
- // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
- nTimer =0;
- /*
- myTimer.start(); // avvia il timer per verificare la presenza del sensore
- while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
- {
- // misura il tempo passato
- nTimer=myTimer.read_us();
- }
- */
- while(myProx ==0)
- {}
- myTimer.stop(); // in ogni caso ferma il timer
- // vai avanti solo se il sensore ha risposto
- //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
- {
- myTimer.start();
- nTimerStart = myTimer.read_us();
- while(myProx == 1)
- {}
- myTimer.stop();
- nTimerStop = myTimer.read_us();
-
-
- // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
- // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
- // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
- fDistance = (nTimerStop-nTimerStart)/58.2;
- // invia il dato al PC
- //pc.printf("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
-
- }
- //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
-
- //++++++++++++++ INIZIO Suona Clacson +++++++++
- //escludi le misure oltre il max
- if((fDistance <= 50.0) && (fDistance >= 3))
- {
- // visualizza il valore misurato
- printf("The Distance was %f [cm]\n\r", fDistance);
-
- // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
- if(fDistance < 22)
- {
- // INIZIO generazione tono
- nIndex=0;
- //Genera il suono del clacson
- for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
- {
- OutWave.write_u16(usaSine[nIndex]); //max 32767
- //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
- wait(fDeltaT);
- // genera ciclicamente
- nIndex++;
- if(nIndex >= SAMPLESINENUM)
- {
- nIndex=0;
- }
- // a metà genera un wait per doppio clacson
- if(nSampleCount == 2000)
- {
- wait_ms(100);
- }
-
- }
- //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
- OutWave.write_u16(0);
-
- } // if(fDistance < soglia) suona clacson
-
- } // if( (fDistance < Max) && (fDistance > Min))
- //++++++++++++++ FINE Suona Clacson +++++++++
- wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
-
- }
- //++++ FINE Ciclo Principale ++++
-}
-
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotFinale_Rev1.cpp Thu Feb 14 16:33:38 2019 +0000
@@ -0,0 +1,423 @@
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Component specific header files. */
+#include "L6208.h"
+
+// numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
+#define SAMPLESINENUM 45 // consigliabile avere multipli di 45
+
+// numero di campioni acquisiti su cui effettuare la media di luminosità
+#define NUMLIGHTSAMPLE 100
+
+// parametri dell'onda coseno da generare
+#define PI (3.141592653589793238462)
+#define AMPLITUDE 32767 //(1.0) // x * 3.3V
+#define PHASE (PI/2) // 2*pi è un periodo
+#define OFFSET 32767 //(0x7FFF)
+
+// Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
+#define SOGLIALUCI (1.5)
+
+// Definizione Parametri per il motore
+#ifdef TARGET_NUCLEO_F334R8
+#define VREFA_PWM_PIN D11
+#define VREFB_PWM_PIN D9
+#elif TARGET_NUCLEO_F302R8
+#define VREFA_PWM_PIN D11
+#define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
+#else
+#define VREFA_PWM_PIN D3
+#define VREFB_PWM_PIN D9
+#endif
+
+// definizioni funzioni di default su scheda
+Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
+DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
+DigitalOut led2(LED2);// LED verde sulla scheda
+
+// pin A2 di output per la forma d'onda analogica dedicata al suono
+AnalogOut OutWave(PA_4);
+
+// pin A1 di input per la forma d'onda analogica dedicata alla luminosità
+AnalogIn InWaveLight(PA_1);
+
+// Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
+DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0);
+// Timer per il calcolo dei tempi del sensore di prossimità
+Timer myTimer;
+
+// tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
+int nTimerStart, nTimer, nTimerStop;
+
+// distanza in cm dell'ostacolo
+double fDistance;
+
+// Buffer contenente la sinusoide da porre in output.
+unsigned short usaSine[SAMPLESINENUM];
+
+// prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
+void CalculateSinewave(void);
+
+
+// Periodo di generazione campioni in output DeltaT = T/NumSample
+double fDeltaT;
+// amplificazione per il dato da spedire sull'ADC
+double fAmp;
+
+// frequenza segnale audio da generare
+double fFreq;
+
+// periodo della sinusoide audio da generare
+double fPeriod;
+
+// indice per i cicli
+int nIndex;
+
+// numero di campioni di onda sonora già generati
+int nSampleCount;
+
+
+// valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
+double fAvgLight;
+
+// valore numerico, di tensione e di luce letto dall'ADC
+volatile unsigned short usReadADC;
+volatile float fReadVoltage;
+
+// valore di luminosità letto dall'ADC
+volatile float fLight;
+
+// Output Digitali usati per i LED
+DigitalOut LedWAD (PC_2);
+DigitalOut LedWAS (PC_3);
+DigitalOut LedWPD (PH_0);
+DigitalOut LedWPS (PA_0) ;
+//DigitalOut LedYAD (PC_13);
+//DigitalOut LedYAS (PC_14);
+DigitalOut LedRPD (PA_13);
+DigitalOut LedRPS (PA_14) ;
+
+// Input/Output Digitali usati per interfaccia RPI
+DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
+DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
+DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
+DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
+DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione = RPI GPIO12
+
+/****************************************************************************/
+/* Initialization parameters of the motor connected to the expansion board. */
+/****************************************************************************/
+l6208_init_t init =
+{
+
+ 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+ 20, //Acceleration current torque in % (from 0 to 100)
+ 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
+ 30, //Deceleration current torque in % (from 0 to 100)
+ 1500, //Running speed in step/s or (1/16)th step/s for microstep modes
+ 50, //Running current torque in % (from 0 to 100)
+ 20, //Holding current torque in % (from 0 to 100)
+ STEP_MODE_1_16, //Step mode via enum motorStepMode_t
+ FAST_DECAY, //Decay mode via enum motorDecayMode_t
+ 0, //Dwelling time in ms
+ FALSE, //Automatic HIZ STOP
+ 100000 //VREFA and VREFB PWM frequency (Hz)
+};
+
+/* Motor Control Component. */
+L6208 *motor;
+
+
+//****************************
+// Create the sinewave buffer
+//****************************
+void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
+{
+ // variabile contenente l'angolo in radianti
+ double fRads;
+ // indici per i cicli
+ int nIndex;
+ // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
+ double fDeltaF;
+ // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
+ double fAngle;
+
+ fDeltaF = 360.0/SAMPLESINENUM;
+ for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++)
+ {
+ fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
+ fRads = (PI * fAngle)/180.0; // Convert degree in radian
+ //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
+ usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
+ }
+}
+
+/********************************************************/
+/* Funzione avviata all'inizio come saluto e Benvenuto */
+/********************************************************/
+void WelcomeMessage()
+{
+}
+
+/********************************************************************/
+/* brief This is an example of user handler for the flag interrupt.*/
+/* param None */
+/* retval None */
+/* note If needed, implement it, and then attach and enable it: */
+/* + motor->attach_flag_irq(&my_flag_irq_handler); */
+/* + motor->enable_flag_irq(); */
+/* To disable it: */
+/* + motor->DisbleFlagIRQ(); */
+/*********************************************************************/
+void my_flag_irq_handler(void)
+{
+ pc.printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
+ motor->disable();
+ pc.printf(" Motor disabled.\r\n\n");
+}
+
+/***************************************************************
+ * brief This is an example of error handler.
+ * param[in] error Number of the error
+ * retval None
+ * note If needed, implement it, and then attach it:
+ * + motor->attach_error_handler(&my_error_handler);
+ **************************************************************/
+void my_error_handler(uint16_t error)
+{
+ /* Printing to the console. */
+ pc.printf("Error %d detected\r\n\n", error);
+
+ /* Infinite loop */
+ while (true)
+ {}
+}
+
+/********/
+/* Main */
+/********/
+int main()
+{
+ // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
+ pc.baud(921600); //921600 bps
+ //pc.baud(9600); //256000 bps
+ pc.printf("*** Test Motor ***\n\r");
+
+ /* Initializing Motor Control Component. */
+ motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
+ if (motor->init(&init) != COMPONENT_OK)
+ {
+ exit(EXIT_FAILURE);
+ }
+
+ /* Attaching and enabling an interrupt handler. */
+ motor->attach_flag_irq(&my_flag_irq_handler);
+ motor->enable_flag_irq();
+
+ /* Attaching an error handler */
+ motor->attach_error_handler(&my_error_handler);
+
+ /* Printing to the console. */
+ pc.printf("Motor Control Application Example for 1 Motor\r\n");
+
+ //----- run the motor BACKWARD
+ pc.printf("--> Running the motor backward.\r\n");
+ motor->run(StepperMotor::BWD);
+
+
+ //----- Soft stop required while running
+ pc.printf("--> Soft stop requested.\r\n");
+ motor->soft_stop();
+
+
+ //----- Change step mode to full step mode
+ motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
+ pc.printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->get_step_mode());
+
+ /* Get current position of device and print to the console */
+ pc.printf(" Position: %d.\r\n", motor->get_position());
+
+ /* Set speed, acceleration and deceleration to scale with normal mode */
+ motor->set_max_speed(init.maxSpeedSps>>4);
+ motor->set_acceleration(motor->get_acceleration()>>4);
+ motor->set_deceleration(motor->get_deceleration()>>4);
+ /* Print parameters to the console */
+ pc.printf(" Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
+ pc.printf(" Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
+ pc.printf(" Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
+ pc.printf(" Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
+
+ //----- move of 200 steps in the FW direction
+ pc.printf("--> Moving forward 200 steps.\r\n");
+ motor->move(StepperMotor::FWD, 200);
+
+ //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
+ fFreq = 440.0; // frequenza in Hz del tono da generare
+ fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
+ fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
+ CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
+ //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
+
+ /*
+ while(true)
+ {
+
+ }
+ */
+
+ //++++ INIZIO Ciclo Principale ++++
+ while (true)
+ {
+ //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
+ if(InMotorSwitchRPI==1)
+ {
+ // Request device to go position -3200
+ motor->go_to(150);
+ // Waiting while the motor is active.
+ motor->wait_while_active();
+ }
+ else
+ {
+ // Request device to go position -3200
+ motor->go_to(-150);
+ // Waiting while the motor is active.
+ motor->wait_while_active();
+ }
+ //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
+ //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
+ if(InLightSwitchRPI ==1)
+ {
+ // accendi i LED di abbellimento
+ led2=1;
+ }
+ else
+ {
+ // spegni i LED di abbellimento
+ led2=0;
+ }
+ //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
+ //++++++++++++ INIZIO Misura della Luminosità e accensione LED ++++++++++++++
+ // inizializza il valore medio della Luminosità
+ fAvgLight=0.0;
+ for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
+ {
+ // acquisisce dato da ADC
+ usReadADC = InWaveLight.read_u16();
+ fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
+ //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
+ fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
+ fAvgLight+=fLight;
+ }
+ // calcola valore medio su NUMSAMPLE acquisizioni
+ fAvgLight/= NUMLIGHTSAMPLE;
+ // Accendi LED Bianchi se illuminazione è sottosoglia
+ if(fAvgLight < SOGLIALUCI)
+ {
+ led2 = 1;
+ }
+ else
+ {
+ led2 = 0;
+ }
+
+ // invia il dato al PC
+ //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
+ //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
+
+
+ //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
+ //inizializza misura di distanza
+ fDistance=0.0;
+ // Fissa come Output il pin myProx
+ myProx.output();
+ // Poni 'L' sul Pin e mantienilo per qualche microsecondo
+ myProx.write(0);
+ wait_us(5);
+ // Poni 'H' sul Pin e mantienilo per qualche microsecondo
+ myProx.write(1);
+ wait_us(10);
+ // Poni 'L' sul Pin e mantienilo per qualche microsecondo
+ myProx.write(0);
+ // Attendi assestamento e Fissa come Input il pin myProx
+ wait_us(5);
+ myProx.input();
+
+ // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
+ nTimer =0;
+ /*
+ myTimer.start(); // avvia il timer per verificare la presenza del sensore
+ while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
+ {
+ // misura il tempo passato
+ nTimer=myTimer.read_us();
+ }
+ */
+ while(myProx ==0)
+ {}
+ myTimer.stop(); // in ogni caso ferma il timer
+ // vai avanti solo se il sensore ha risposto
+ //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
+ {
+ myTimer.start();
+ nTimerStart = myTimer.read_us();
+ while(myProx == 1)
+ {}
+ myTimer.stop();
+ nTimerStop = myTimer.read_us();
+
+
+ // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
+ // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
+ // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
+ fDistance = (nTimerStop-nTimerStart)/58.2;
+ // invia il dato al PC
+ //pc.printf("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
+
+ }
+ //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
+
+ //++++++++++++++ INIZIO Suona Clacson +++++++++
+ //escludi le misure oltre il max
+ if((fDistance <= 50.0) && (fDistance >= 3))
+ {
+ // visualizza il valore misurato
+ printf("The Distance was %f [cm]\n\r", fDistance);
+
+ // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
+ if(fDistance < 22)
+ {
+ // INIZIO generazione tono
+ nIndex=0;
+ //Genera il suono del clacson
+ for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
+ {
+ OutWave.write_u16(usaSine[nIndex]); //max 32767
+ //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
+ wait(fDeltaT);
+ // genera ciclicamente
+ nIndex++;
+ if(nIndex >= SAMPLESINENUM)
+ {
+ nIndex=0;
+ }
+ // a metà genera un wait per doppio clacson
+ if(nSampleCount == 2000)
+ {
+ wait_ms(100);
+ }
+
+ }
+ //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
+ OutWave.write_u16(0);
+
+ } // if(fDistance < soglia) suona clacson
+
+ } // if( (fDistance < Max) && (fDistance > Min))
+ //++++++++++++++ FINE Suona Clacson +++++++++
+ wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
+
+ }
+ //++++ FINE Ciclo Principale ++++
+}
+