Amaldi / Mbed 2 deprecated Amaldi_RobotFinale_Rev2-4_FUNZIONA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

RobotFinale.cpp@13:459e008582b3, 2018-11-24 (annotated)

Committer:
pinofal
Date:
Sat Nov 24 13:30:24 2018 +0000
Revision:
13:459e008582b3
Parent:
12:00ce5d30b82c
Child:
14:d58d474fae35
Amaldi Robot Finale rev 2.0

Who changed what in which revision?

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