Amaldi / Mbed 2 deprecated Amaldi_RobotFinale_Rev1_FUNZIONA

Amaldi Robot Ver. 1.0

Dependencies:   mbed X-NUCLEO-IHM05A1

RobotFinale_Rev1.cpp@13:24b2e46b0fcf, 2019-02-14 (annotated)

Committer:
pinofal
Date:
Thu Feb 14 16:33:38 2019 +0000
Revision:
13:24b2e46b0fcf
Robot Rev 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pinofal 13:24b2e46b0fcf 1 /* mbed specific header files. */
pinofal 13:24b2e46b0fcf 2 #include "mbed.h"
pinofal 13:24b2e46b0fcf 3
pinofal 13:24b2e46b0fcf 4 /* Component specific header files. */
pinofal 13:24b2e46b0fcf 5 #include "L6208.h"
pinofal 13:24b2e46b0fcf 6
pinofal 13:24b2e46b0fcf 7 // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
pinofal 13:24b2e46b0fcf 8 #define SAMPLESINENUM 45 // consigliabile avere multipli di 45
pinofal 13:24b2e46b0fcf 9
pinofal 13:24b2e46b0fcf 10 // numero di campioni acquisiti su cui effettuare la media di luminosità
pinofal 13:24b2e46b0fcf 11 #define NUMLIGHTSAMPLE 100
pinofal 13:24b2e46b0fcf 12
pinofal 13:24b2e46b0fcf 13 // parametri dell'onda coseno da generare
pinofal 13:24b2e46b0fcf 14 #define PI (3.141592653589793238462)
pinofal 13:24b2e46b0fcf 15 #define AMPLITUDE 32767 //(1.0) // x * 3.3V
pinofal 13:24b2e46b0fcf 16 #define PHASE (PI/2) // 2*pi è un periodo
pinofal 13:24b2e46b0fcf 17 #define OFFSET 32767 //(0x7FFF)
pinofal 13:24b2e46b0fcf 18
pinofal 13:24b2e46b0fcf 19 // Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
pinofal 13:24b2e46b0fcf 20 #define SOGLIALUCI (1.5)
pinofal 13:24b2e46b0fcf 21
pinofal 13:24b2e46b0fcf 22 // Definizione Parametri per il motore
pinofal 13:24b2e46b0fcf 23 #ifdef TARGET_NUCLEO_F334R8
pinofal 13:24b2e46b0fcf 24 #define VREFA_PWM_PIN D11
pinofal 13:24b2e46b0fcf 25 #define VREFB_PWM_PIN D9
pinofal 13:24b2e46b0fcf 26 #elif TARGET_NUCLEO_F302R8
pinofal 13:24b2e46b0fcf 27 #define VREFA_PWM_PIN D11
pinofal 13:24b2e46b0fcf 28 #define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
pinofal 13:24b2e46b0fcf 29 #else
pinofal 13:24b2e46b0fcf 30 #define VREFA_PWM_PIN D3
pinofal 13:24b2e46b0fcf 31 #define VREFB_PWM_PIN D9
pinofal 13:24b2e46b0fcf 32 #endif
pinofal 13:24b2e46b0fcf 33
pinofal 13:24b2e46b0fcf 34 // definizioni funzioni di default su scheda
pinofal 13:24b2e46b0fcf 35 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
pinofal 13:24b2e46b0fcf 36 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
pinofal 13:24b2e46b0fcf 37 DigitalOut led2(LED2);// LED verde sulla scheda
pinofal 13:24b2e46b0fcf 38
pinofal 13:24b2e46b0fcf 39 // pin A2 di output per la forma d'onda analogica dedicata al suono
pinofal 13:24b2e46b0fcf 40 AnalogOut OutWave(PA_4);
pinofal 13:24b2e46b0fcf 41
pinofal 13:24b2e46b0fcf 42 // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
pinofal 13:24b2e46b0fcf 43 AnalogIn InWaveLight(PA_1);
pinofal 13:24b2e46b0fcf 44
pinofal 13:24b2e46b0fcf 45 // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
pinofal 13:24b2e46b0fcf 46 DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0);
pinofal 13:24b2e46b0fcf 47 // Timer per il calcolo dei tempi del sensore di prossimità
pinofal 13:24b2e46b0fcf 48 Timer myTimer;
pinofal 13:24b2e46b0fcf 49
pinofal 13:24b2e46b0fcf 50 // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
pinofal 13:24b2e46b0fcf 51 int nTimerStart, nTimer, nTimerStop;
pinofal 13:24b2e46b0fcf 52
pinofal 13:24b2e46b0fcf 53 // distanza in cm dell'ostacolo
pinofal 13:24b2e46b0fcf 54 double fDistance;
pinofal 13:24b2e46b0fcf 55
pinofal 13:24b2e46b0fcf 56 // Buffer contenente la sinusoide da porre in output.
pinofal 13:24b2e46b0fcf 57 unsigned short usaSine[SAMPLESINENUM];
pinofal 13:24b2e46b0fcf 58
pinofal 13:24b2e46b0fcf 59 // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
pinofal 13:24b2e46b0fcf 60 void CalculateSinewave(void);
pinofal 13:24b2e46b0fcf 61
pinofal 13:24b2e46b0fcf 62
pinofal 13:24b2e46b0fcf 63 // Periodo di generazione campioni in output DeltaT = T/NumSample
pinofal 13:24b2e46b0fcf 64 double fDeltaT;
pinofal 13:24b2e46b0fcf 65 // amplificazione per il dato da spedire sull'ADC
pinofal 13:24b2e46b0fcf 66 double fAmp;
pinofal 13:24b2e46b0fcf 67
pinofal 13:24b2e46b0fcf 68 // frequenza segnale audio da generare
pinofal 13:24b2e46b0fcf 69 double fFreq;
pinofal 13:24b2e46b0fcf 70
pinofal 13:24b2e46b0fcf 71 // periodo della sinusoide audio da generare
pinofal 13:24b2e46b0fcf 72 double fPeriod;
pinofal 13:24b2e46b0fcf 73
pinofal 13:24b2e46b0fcf 74 // indice per i cicli
pinofal 13:24b2e46b0fcf 75 int nIndex;
pinofal 13:24b2e46b0fcf 76
pinofal 13:24b2e46b0fcf 77 // numero di campioni di onda sonora già generati
pinofal 13:24b2e46b0fcf 78 int nSampleCount;
pinofal 13:24b2e46b0fcf 79
pinofal 13:24b2e46b0fcf 80
pinofal 13:24b2e46b0fcf 81 // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
pinofal 13:24b2e46b0fcf 82 double fAvgLight;
pinofal 13:24b2e46b0fcf 83
pinofal 13:24b2e46b0fcf 84 // valore numerico, di tensione e di luce letto dall'ADC
pinofal 13:24b2e46b0fcf 85 volatile unsigned short usReadADC;
pinofal 13:24b2e46b0fcf 86 volatile float fReadVoltage;
pinofal 13:24b2e46b0fcf 87
pinofal 13:24b2e46b0fcf 88 // valore di luminosità letto dall'ADC
pinofal 13:24b2e46b0fcf 89 volatile float fLight;
pinofal 13:24b2e46b0fcf 90
pinofal 13:24b2e46b0fcf 91 // Output Digitali usati per i LED
pinofal 13:24b2e46b0fcf 92 DigitalOut LedWAD (PC_2);
pinofal 13:24b2e46b0fcf 93 DigitalOut LedWAS (PC_3);
pinofal 13:24b2e46b0fcf 94 DigitalOut LedWPD (PH_0);
pinofal 13:24b2e46b0fcf 95 DigitalOut LedWPS (PA_0) ;
pinofal 13:24b2e46b0fcf 96 //DigitalOut LedYAD (PC_13);
pinofal 13:24b2e46b0fcf 97 //DigitalOut LedYAS (PC_14);
pinofal 13:24b2e46b0fcf 98 DigitalOut LedRPD (PA_13);
pinofal 13:24b2e46b0fcf 99 DigitalOut LedRPS (PA_14) ;
pinofal 13:24b2e46b0fcf 100
pinofal 13:24b2e46b0fcf 101 // Input/Output Digitali usati per interfaccia RPI
pinofal 13:24b2e46b0fcf 102 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
pinofal 13:24b2e46b0fcf 103 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
pinofal 13:24b2e46b0fcf 104 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
pinofal 13:24b2e46b0fcf 105 DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
pinofal 13:24b2e46b0fcf 106 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione = RPI GPIO12
pinofal 13:24b2e46b0fcf 107
pinofal 13:24b2e46b0fcf 108 /****************************************************************************/
pinofal 13:24b2e46b0fcf 109 /* Initialization parameters of the motor connected to the expansion board. */
pinofal 13:24b2e46b0fcf 110 /****************************************************************************/
pinofal 13:24b2e46b0fcf 111 l6208_init_t init =
pinofal 13:24b2e46b0fcf 112 {
pinofal 13:24b2e46b0fcf 113
pinofal 13:24b2e46b0fcf 114 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
pinofal 13:24b2e46b0fcf 115 20, //Acceleration current torque in % (from 0 to 100)
pinofal 13:24b2e46b0fcf 116 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
pinofal 13:24b2e46b0fcf 117 30, //Deceleration current torque in % (from 0 to 100)
pinofal 13:24b2e46b0fcf 118 1500, //Running speed in step/s or (1/16)th step/s for microstep modes
pinofal 13:24b2e46b0fcf 119 50, //Running current torque in % (from 0 to 100)
pinofal 13:24b2e46b0fcf 120 20, //Holding current torque in % (from 0 to 100)
pinofal 13:24b2e46b0fcf 121 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
pinofal 13:24b2e46b0fcf 122 FAST_DECAY, //Decay mode via enum motorDecayMode_t
pinofal 13:24b2e46b0fcf 123 0, //Dwelling time in ms
pinofal 13:24b2e46b0fcf 124 FALSE, //Automatic HIZ STOP
pinofal 13:24b2e46b0fcf 125 100000 //VREFA and VREFB PWM frequency (Hz)
pinofal 13:24b2e46b0fcf 126 };
pinofal 13:24b2e46b0fcf 127
pinofal 13:24b2e46b0fcf 128 /* Motor Control Component. */
pinofal 13:24b2e46b0fcf 129 L6208 *motor;
pinofal 13:24b2e46b0fcf 130
pinofal 13:24b2e46b0fcf 131
pinofal 13:24b2e46b0fcf 132 //****************************
pinofal 13:24b2e46b0fcf 133 // Create the sinewave buffer
pinofal 13:24b2e46b0fcf 134 //****************************
pinofal 13:24b2e46b0fcf 135 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
pinofal 13:24b2e46b0fcf 136 {
pinofal 13:24b2e46b0fcf 137 // variabile contenente l'angolo in radianti
pinofal 13:24b2e46b0fcf 138 double fRads;
pinofal 13:24b2e46b0fcf 139 // indici per i cicli
pinofal 13:24b2e46b0fcf 140 int nIndex;
pinofal 13:24b2e46b0fcf 141 // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
pinofal 13:24b2e46b0fcf 142 double fDeltaF;
pinofal 13:24b2e46b0fcf 143 // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
pinofal 13:24b2e46b0fcf 144 double fAngle;
pinofal 13:24b2e46b0fcf 145
pinofal 13:24b2e46b0fcf 146 fDeltaF = 360.0/SAMPLESINENUM;
pinofal 13:24b2e46b0fcf 147 for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++)
pinofal 13:24b2e46b0fcf 148 {
pinofal 13:24b2e46b0fcf 149 fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
pinofal 13:24b2e46b0fcf 150 fRads = (PI * fAngle)/180.0; // Convert degree in radian
pinofal 13:24b2e46b0fcf 151 //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
pinofal 13:24b2e46b0fcf 152 usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
pinofal 13:24b2e46b0fcf 153 }
pinofal 13:24b2e46b0fcf 154 }
pinofal 13:24b2e46b0fcf 155
pinofal 13:24b2e46b0fcf 156 /********************************************************/
pinofal 13:24b2e46b0fcf 157 /* Funzione avviata all'inizio come saluto e Benvenuto */
pinofal 13:24b2e46b0fcf 158 /********************************************************/
pinofal 13:24b2e46b0fcf 159 void WelcomeMessage()
pinofal 13:24b2e46b0fcf 160 {
pinofal 13:24b2e46b0fcf 161 }
pinofal 13:24b2e46b0fcf 162
pinofal 13:24b2e46b0fcf 163 /********************************************************************/
pinofal 13:24b2e46b0fcf 164 /* brief This is an example of user handler for the flag interrupt.*/
pinofal 13:24b2e46b0fcf 165 /* param None */
pinofal 13:24b2e46b0fcf 166 /* retval None */
pinofal 13:24b2e46b0fcf 167 /* note If needed, implement it, and then attach and enable it: */
pinofal 13:24b2e46b0fcf 168 /* + motor->attach_flag_irq(&my_flag_irq_handler); */
pinofal 13:24b2e46b0fcf 169 /* + motor->enable_flag_irq(); */
pinofal 13:24b2e46b0fcf 170 /* To disable it: */
pinofal 13:24b2e46b0fcf 171 /* + motor->DisbleFlagIRQ(); */
pinofal 13:24b2e46b0fcf 172 /*********************************************************************/
pinofal 13:24b2e46b0fcf 173 void my_flag_irq_handler(void)
pinofal 13:24b2e46b0fcf 174 {
pinofal 13:24b2e46b0fcf 175 pc.printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
pinofal 13:24b2e46b0fcf 176 motor->disable();
pinofal 13:24b2e46b0fcf 177 pc.printf(" Motor disabled.\r\n\n");
pinofal 13:24b2e46b0fcf 178 }
pinofal 13:24b2e46b0fcf 179
pinofal 13:24b2e46b0fcf 180 /***************************************************************
pinofal 13:24b2e46b0fcf 181 * brief This is an example of error handler.
pinofal 13:24b2e46b0fcf 182 * param[in] error Number of the error
pinofal 13:24b2e46b0fcf 183 * retval None
pinofal 13:24b2e46b0fcf 184 * note If needed, implement it, and then attach it:
pinofal 13:24b2e46b0fcf 185 * + motor->attach_error_handler(&my_error_handler);
pinofal 13:24b2e46b0fcf 186 **************************************************************/
pinofal 13:24b2e46b0fcf 187 void my_error_handler(uint16_t error)
pinofal 13:24b2e46b0fcf 188 {
pinofal 13:24b2e46b0fcf 189 /* Printing to the console. */
pinofal 13:24b2e46b0fcf 190 pc.printf("Error %d detected\r\n\n", error);
pinofal 13:24b2e46b0fcf 191
pinofal 13:24b2e46b0fcf 192 /* Infinite loop */
pinofal 13:24b2e46b0fcf 193 while (true)
pinofal 13:24b2e46b0fcf 194 {}
pinofal 13:24b2e46b0fcf 195 }
pinofal 13:24b2e46b0fcf 196
pinofal 13:24b2e46b0fcf 197 /********/
pinofal 13:24b2e46b0fcf 198 /* Main */
pinofal 13:24b2e46b0fcf 199 /********/
pinofal 13:24b2e46b0fcf 200 int main()
pinofal 13:24b2e46b0fcf 201 {
pinofal 13:24b2e46b0fcf 202 // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
pinofal 13:24b2e46b0fcf 203 pc.baud(921600); //921600 bps
pinofal 13:24b2e46b0fcf 204 //pc.baud(9600); //256000 bps
pinofal 13:24b2e46b0fcf 205 pc.printf("*** Test Motor ***\n\r");
pinofal 13:24b2e46b0fcf 206
pinofal 13:24b2e46b0fcf 207 /* Initializing Motor Control Component. */
pinofal 13:24b2e46b0fcf 208 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
pinofal 13:24b2e46b0fcf 209 if (motor->init(&init) != COMPONENT_OK)
pinofal 13:24b2e46b0fcf 210 {
pinofal 13:24b2e46b0fcf 211 exit(EXIT_FAILURE);
pinofal 13:24b2e46b0fcf 212 }
pinofal 13:24b2e46b0fcf 213
pinofal 13:24b2e46b0fcf 214 /* Attaching and enabling an interrupt handler. */
pinofal 13:24b2e46b0fcf 215 motor->attach_flag_irq(&my_flag_irq_handler);
pinofal 13:24b2e46b0fcf 216 motor->enable_flag_irq();
pinofal 13:24b2e46b0fcf 217
pinofal 13:24b2e46b0fcf 218 /* Attaching an error handler */
pinofal 13:24b2e46b0fcf 219 motor->attach_error_handler(&my_error_handler);
pinofal 13:24b2e46b0fcf 220
pinofal 13:24b2e46b0fcf 221 /* Printing to the console. */
pinofal 13:24b2e46b0fcf 222 pc.printf("Motor Control Application Example for 1 Motor\r\n");
pinofal 13:24b2e46b0fcf 223
pinofal 13:24b2e46b0fcf 224 //----- run the motor BACKWARD
pinofal 13:24b2e46b0fcf 225 pc.printf("--> Running the motor backward.\r\n");
pinofal 13:24b2e46b0fcf 226 motor->run(StepperMotor::BWD);
pinofal 13:24b2e46b0fcf 227
pinofal 13:24b2e46b0fcf 228
pinofal 13:24b2e46b0fcf 229 //----- Soft stop required while running
pinofal 13:24b2e46b0fcf 230 pc.printf("--> Soft stop requested.\r\n");
pinofal 13:24b2e46b0fcf 231 motor->soft_stop();
pinofal 13:24b2e46b0fcf 232
pinofal 13:24b2e46b0fcf 233
pinofal 13:24b2e46b0fcf 234 //----- Change step mode to full step mode
pinofal 13:24b2e46b0fcf 235 motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
pinofal 13:24b2e46b0fcf 236 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());
pinofal 13:24b2e46b0fcf 237
pinofal 13:24b2e46b0fcf 238 /* Get current position of device and print to the console */
pinofal 13:24b2e46b0fcf 239 pc.printf(" Position: %d.\r\n", motor->get_position());
pinofal 13:24b2e46b0fcf 240
pinofal 13:24b2e46b0fcf 241 /* Set speed, acceleration and deceleration to scale with normal mode */
pinofal 13:24b2e46b0fcf 242 motor->set_max_speed(init.maxSpeedSps>>4);
pinofal 13:24b2e46b0fcf 243 motor->set_acceleration(motor->get_acceleration()>>4);
pinofal 13:24b2e46b0fcf 244 motor->set_deceleration(motor->get_deceleration()>>4);
pinofal 13:24b2e46b0fcf 245 /* Print parameters to the console */
pinofal 13:24b2e46b0fcf 246 pc.printf(" Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
pinofal 13:24b2e46b0fcf 247 pc.printf(" Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
pinofal 13:24b2e46b0fcf 248 pc.printf(" Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
pinofal 13:24b2e46b0fcf 249 pc.printf(" Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
pinofal 13:24b2e46b0fcf 250
pinofal 13:24b2e46b0fcf 251 //----- move of 200 steps in the FW direction
pinofal 13:24b2e46b0fcf 252 pc.printf("--> Moving forward 200 steps.\r\n");
pinofal 13:24b2e46b0fcf 253 motor->move(StepperMotor::FWD, 200);
pinofal 13:24b2e46b0fcf 254
pinofal 13:24b2e46b0fcf 255 //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
pinofal 13:24b2e46b0fcf 256 fFreq = 440.0; // frequenza in Hz del tono da generare
pinofal 13:24b2e46b0fcf 257 fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
pinofal 13:24b2e46b0fcf 258 fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
pinofal 13:24b2e46b0fcf 259 CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
pinofal 13:24b2e46b0fcf 260 //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
pinofal 13:24b2e46b0fcf 261
pinofal 13:24b2e46b0fcf 262 /*
pinofal 13:24b2e46b0fcf 263 while(true)
pinofal 13:24b2e46b0fcf 264 {
pinofal 13:24b2e46b0fcf 265
pinofal 13:24b2e46b0fcf 266 }
pinofal 13:24b2e46b0fcf 267 */
pinofal 13:24b2e46b0fcf 268
pinofal 13:24b2e46b0fcf 269 //++++ INIZIO Ciclo Principale ++++
pinofal 13:24b2e46b0fcf 270 while (true)
pinofal 13:24b2e46b0fcf 271 {
pinofal 13:24b2e46b0fcf 272 //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
pinofal 13:24b2e46b0fcf 273 if(InMotorSwitchRPI==1)
pinofal 13:24b2e46b0fcf 274 {
pinofal 13:24b2e46b0fcf 275 // Request device to go position -3200
pinofal 13:24b2e46b0fcf 276 motor->go_to(150);
pinofal 13:24b2e46b0fcf 277 // Waiting while the motor is active.
pinofal 13:24b2e46b0fcf 278 motor->wait_while_active();
pinofal 13:24b2e46b0fcf 279 }
pinofal 13:24b2e46b0fcf 280 else
pinofal 13:24b2e46b0fcf 281 {
pinofal 13:24b2e46b0fcf 282 // Request device to go position -3200
pinofal 13:24b2e46b0fcf 283 motor->go_to(-150);
pinofal 13:24b2e46b0fcf 284 // Waiting while the motor is active.
pinofal 13:24b2e46b0fcf 285 motor->wait_while_active();
pinofal 13:24b2e46b0fcf 286 }
pinofal 13:24b2e46b0fcf 287 //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
pinofal 13:24b2e46b0fcf 288 //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
pinofal 13:24b2e46b0fcf 289 if(InLightSwitchRPI ==1)
pinofal 13:24b2e46b0fcf 290 {
pinofal 13:24b2e46b0fcf 291 // accendi i LED di abbellimento
pinofal 13:24b2e46b0fcf 292 led2=1;
pinofal 13:24b2e46b0fcf 293 }
pinofal 13:24b2e46b0fcf 294 else
pinofal 13:24b2e46b0fcf 295 {
pinofal 13:24b2e46b0fcf 296 // spegni i LED di abbellimento
pinofal 13:24b2e46b0fcf 297 led2=0;
pinofal 13:24b2e46b0fcf 298 }
pinofal 13:24b2e46b0fcf 299 //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
pinofal 13:24b2e46b0fcf 300 //++++++++++++ INIZIO Misura della Luminosità e accensione LED ++++++++++++++
pinofal 13:24b2e46b0fcf 301 // inizializza il valore medio della Luminosità
pinofal 13:24b2e46b0fcf 302 fAvgLight=0.0;
pinofal 13:24b2e46b0fcf 303 for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
pinofal 13:24b2e46b0fcf 304 {
pinofal 13:24b2e46b0fcf 305 // acquisisce dato da ADC
pinofal 13:24b2e46b0fcf 306 usReadADC = InWaveLight.read_u16();
pinofal 13:24b2e46b0fcf 307 fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
pinofal 13:24b2e46b0fcf 308 //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
pinofal 13:24b2e46b0fcf 309 fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
pinofal 13:24b2e46b0fcf 310 fAvgLight+=fLight;
pinofal 13:24b2e46b0fcf 311 }
pinofal 13:24b2e46b0fcf 312 // calcola valore medio su NUMSAMPLE acquisizioni
pinofal 13:24b2e46b0fcf 313 fAvgLight/= NUMLIGHTSAMPLE;
pinofal 13:24b2e46b0fcf 314 // Accendi LED Bianchi se illuminazione è sottosoglia
pinofal 13:24b2e46b0fcf 315 if(fAvgLight < SOGLIALUCI)
pinofal 13:24b2e46b0fcf 316 {
pinofal 13:24b2e46b0fcf 317 led2 = 1;
pinofal 13:24b2e46b0fcf 318 }
pinofal 13:24b2e46b0fcf 319 else
pinofal 13:24b2e46b0fcf 320 {
pinofal 13:24b2e46b0fcf 321 led2 = 0;
pinofal 13:24b2e46b0fcf 322 }
pinofal 13:24b2e46b0fcf 323
pinofal 13:24b2e46b0fcf 324 // invia il dato al PC
pinofal 13:24b2e46b0fcf 325 //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
pinofal 13:24b2e46b0fcf 326 //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
pinofal 13:24b2e46b0fcf 327
pinofal 13:24b2e46b0fcf 328
pinofal 13:24b2e46b0fcf 329 //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
pinofal 13:24b2e46b0fcf 330 //inizializza misura di distanza
pinofal 13:24b2e46b0fcf 331 fDistance=0.0;
pinofal 13:24b2e46b0fcf 332 // Fissa come Output il pin myProx
pinofal 13:24b2e46b0fcf 333 myProx.output();
pinofal 13:24b2e46b0fcf 334 // Poni 'L' sul Pin e mantienilo per qualche microsecondo
pinofal 13:24b2e46b0fcf 335 myProx.write(0);
pinofal 13:24b2e46b0fcf 336 wait_us(5);
pinofal 13:24b2e46b0fcf 337 // Poni 'H' sul Pin e mantienilo per qualche microsecondo
pinofal 13:24b2e46b0fcf 338 myProx.write(1);
pinofal 13:24b2e46b0fcf 339 wait_us(10);
pinofal 13:24b2e46b0fcf 340 // Poni 'L' sul Pin e mantienilo per qualche microsecondo
pinofal 13:24b2e46b0fcf 341 myProx.write(0);
pinofal 13:24b2e46b0fcf 342 // Attendi assestamento e Fissa come Input il pin myProx
pinofal 13:24b2e46b0fcf 343 wait_us(5);
pinofal 13:24b2e46b0fcf 344 myProx.input();
pinofal 13:24b2e46b0fcf 345
pinofal 13:24b2e46b0fcf 346 // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
pinofal 13:24b2e46b0fcf 347 nTimer =0;
pinofal 13:24b2e46b0fcf 348 /*
pinofal 13:24b2e46b0fcf 349 myTimer.start(); // avvia il timer per verificare la presenza del sensore
pinofal 13:24b2e46b0fcf 350 while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
pinofal 13:24b2e46b0fcf 351 {
pinofal 13:24b2e46b0fcf 352 // misura il tempo passato
pinofal 13:24b2e46b0fcf 353 nTimer=myTimer.read_us();
pinofal 13:24b2e46b0fcf 354 }
pinofal 13:24b2e46b0fcf 355 */
pinofal 13:24b2e46b0fcf 356 while(myProx ==0)
pinofal 13:24b2e46b0fcf 357 {}
pinofal 13:24b2e46b0fcf 358 myTimer.stop(); // in ogni caso ferma il timer
pinofal 13:24b2e46b0fcf 359 // vai avanti solo se il sensore ha risposto
pinofal 13:24b2e46b0fcf 360 //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
pinofal 13:24b2e46b0fcf 361 {
pinofal 13:24b2e46b0fcf 362 myTimer.start();
pinofal 13:24b2e46b0fcf 363 nTimerStart = myTimer.read_us();
pinofal 13:24b2e46b0fcf 364 while(myProx == 1)
pinofal 13:24b2e46b0fcf 365 {}
pinofal 13:24b2e46b0fcf 366 myTimer.stop();
pinofal 13:24b2e46b0fcf 367 nTimerStop = myTimer.read_us();
pinofal 13:24b2e46b0fcf 368
pinofal 13:24b2e46b0fcf 369
pinofal 13:24b2e46b0fcf 370 // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
pinofal 13:24b2e46b0fcf 371 // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
pinofal 13:24b2e46b0fcf 372 // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
pinofal 13:24b2e46b0fcf 373 fDistance = (nTimerStop-nTimerStart)/58.2;
pinofal 13:24b2e46b0fcf 374 // invia il dato al PC
pinofal 13:24b2e46b0fcf 375 //pc.printf("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
pinofal 13:24b2e46b0fcf 376
pinofal 13:24b2e46b0fcf 377 }
pinofal 13:24b2e46b0fcf 378 //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
pinofal 13:24b2e46b0fcf 379
pinofal 13:24b2e46b0fcf 380 //++++++++++++++ INIZIO Suona Clacson +++++++++
pinofal 13:24b2e46b0fcf 381 //escludi le misure oltre il max
pinofal 13:24b2e46b0fcf 382 if((fDistance <= 50.0) && (fDistance >= 3))
pinofal 13:24b2e46b0fcf 383 {
pinofal 13:24b2e46b0fcf 384 // visualizza il valore misurato
pinofal 13:24b2e46b0fcf 385 printf("The Distance was %f [cm]\n\r", fDistance);
pinofal 13:24b2e46b0fcf 386
pinofal 13:24b2e46b0fcf 387 // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
pinofal 13:24b2e46b0fcf 388 if(fDistance < 22)
pinofal 13:24b2e46b0fcf 389 {
pinofal 13:24b2e46b0fcf 390 // INIZIO generazione tono
pinofal 13:24b2e46b0fcf 391 nIndex=0;
pinofal 13:24b2e46b0fcf 392 //Genera il suono del clacson
pinofal 13:24b2e46b0fcf 393 for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
pinofal 13:24b2e46b0fcf 394 {
pinofal 13:24b2e46b0fcf 395 OutWave.write_u16(usaSine[nIndex]); //max 32767
pinofal 13:24b2e46b0fcf 396 //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
pinofal 13:24b2e46b0fcf 397 wait(fDeltaT);
pinofal 13:24b2e46b0fcf 398 // genera ciclicamente
pinofal 13:24b2e46b0fcf 399 nIndex++;
pinofal 13:24b2e46b0fcf 400 if(nIndex >= SAMPLESINENUM)
pinofal 13:24b2e46b0fcf 401 {
pinofal 13:24b2e46b0fcf 402 nIndex=0;
pinofal 13:24b2e46b0fcf 403 }
pinofal 13:24b2e46b0fcf 404 // a metà genera un wait per doppio clacson
pinofal 13:24b2e46b0fcf 405 if(nSampleCount == 2000)
pinofal 13:24b2e46b0fcf 406 {
pinofal 13:24b2e46b0fcf 407 wait_ms(100);
pinofal 13:24b2e46b0fcf 408 }
pinofal 13:24b2e46b0fcf 409
pinofal 13:24b2e46b0fcf 410 }
pinofal 13:24b2e46b0fcf 411 //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
pinofal 13:24b2e46b0fcf 412 OutWave.write_u16(0);
pinofal 13:24b2e46b0fcf 413
pinofal 13:24b2e46b0fcf 414 } // if(fDistance < soglia) suona clacson
pinofal 13:24b2e46b0fcf 415
pinofal 13:24b2e46b0fcf 416 } // if( (fDistance < Max) && (fDistance > Min))
pinofal 13:24b2e46b0fcf 417 //++++++++++++++ FINE Suona Clacson +++++++++
pinofal 13:24b2e46b0fcf 418 wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
pinofal 13:24b2e46b0fcf 419
pinofal 13:24b2e46b0fcf 420 }
pinofal 13:24b2e46b0fcf 421 //++++ FINE Ciclo Principale ++++
pinofal 13:24b2e46b0fcf 422 }
pinofal 13:24b2e46b0fcf 423