Amaldi
/
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@14:d58d474fae35, 2019-02-14 (annotated)
- Committer:
- pinofal
- Date:
- Thu Feb 14 16:32:40 2019 +0000
- Revision:
- 14:d58d474fae35
- Parent:
- 13:459e008582b3
Robot rev 2.4
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| pinofal | 14:d58d474fae35 | 1 | // mbed specific header files. |
| pinofal | 12:00ce5d30b82c | 2 | #include "mbed.h" |
| pinofal | 12:00ce5d30b82c | 3 | |
| pinofal | 14:d58d474fae35 | 4 | // include suono del motore |
| pinofal | 14:d58d474fae35 | 5 | #include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti |
| pinofal | 14:d58d474fae35 | 6 | //#include "Gurgle.h" |
| pinofal | 14:d58d474fae35 | 7 | #include "SampledSoundWelcome.h" // messaggio di benvenuto |
| pinofal | 14:d58d474fae35 | 8 | #include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia |
| pinofal | 14:d58d474fae35 | 9 | |
| pinofal | 14:d58d474fae35 | 10 | //#include "SampledSoundMotosega.h" |
| pinofal | 14:d58d474fae35 | 11 | //#include "SampledSoundTrattore.h" |
| pinofal | 14:d58d474fae35 | 12 | |
| pinofal | 14:d58d474fae35 | 13 | |
| pinofal | 14:d58d474fae35 | 14 | // TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR |
| pinofal | 14:d58d474fae35 | 15 | #define TIMEOUTPROXSENSOR 1000 //tempo in [microsec] |
| pinofal | 12:00ce5d30b82c | 16 | |
| pinofal | 12:00ce5d30b82c | 17 | // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC |
| pinofal | 14:d58d474fae35 | 18 | #define CLACSONSAMPLENUM 45 // consigliabile avere multipli di 45 |
| pinofal | 12:00ce5d30b82c | 19 | |
| pinofal | 12:00ce5d30b82c | 20 | // numero di campioni acquisiti su cui effettuare la media di luminosità |
| pinofal | 12:00ce5d30b82c | 21 | #define NUMLIGHTSAMPLE 100 |
| pinofal | 14:d58d474fae35 | 22 | |
| pinofal | 14:d58d474fae35 | 23 | // Parametri di soglia per la luce. Accendi/spegni Luci se la luminosità scende/sale sotto/sopra SOGLIALUCIMAX e SOGLIALUCIMIN |
| pinofal | 14:d58d474fae35 | 24 | #define SOGLIALUCIMAX (1.85) |
| pinofal | 14:d58d474fae35 | 25 | #define SOGLIALUCIMIN (1.45) |
| pinofal | 14:d58d474fae35 | 26 | |
| pinofal | 12:00ce5d30b82c | 27 | // parametri dell'onda coseno da generare |
| pinofal | 12:00ce5d30b82c | 28 | #define PI (3.141592653589793238462) |
| pinofal | 12:00ce5d30b82c | 29 | #define AMPLITUDE 32767 //(1.0) // x * 3.3V |
| pinofal | 12:00ce5d30b82c | 30 | #define PHASE (PI/2) // 2*pi è un periodo |
| pinofal | 12:00ce5d30b82c | 31 | #define OFFSET 32767 //(0x7FFF) |
| pinofal | 12:00ce5d30b82c | 32 | |
| pinofal | 12:00ce5d30b82c | 33 | |
| pinofal | 14:d58d474fae35 | 34 | // ticker per la generazione dell'onda con DAC |
| pinofal | 14:d58d474fae35 | 35 | Ticker SampleOutTicker; |
| pinofal | 12:00ce5d30b82c | 36 | |
| pinofal | 12:00ce5d30b82c | 37 | |
| pinofal | 12:00ce5d30b82c | 38 | // Timer per il calcolo dei tempi del sensore di prossimità |
| pinofal | 14:d58d474fae35 | 39 | Timer TimerProxSensor; |
| pinofal | 12:00ce5d30b82c | 40 | |
| pinofal | 12:00ce5d30b82c | 41 | // distanza in cm dell'ostacolo |
| pinofal | 12:00ce5d30b82c | 42 | double fDistance; |
| pinofal | 12:00ce5d30b82c | 43 | |
| pinofal | 14:d58d474fae35 | 44 | |
| pinofal | 14:d58d474fae35 | 45 | // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni |
| pinofal | 14:d58d474fae35 | 46 | int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow; |
| pinofal | 14:d58d474fae35 | 47 | |
| pinofal | 14:d58d474fae35 | 48 | // Buffer contenente la sinusoide da porre in output come Clacson. |
| pinofal | 14:d58d474fae35 | 49 | unsigned short usaClacson[CLACSONSAMPLENUM]; |
| pinofal | 12:00ce5d30b82c | 50 | |
| pinofal | 12:00ce5d30b82c | 51 | // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC |
| pinofal | 12:00ce5d30b82c | 52 | void CalculateSinewave(void); |
| pinofal | 12:00ce5d30b82c | 53 | |
| pinofal | 12:00ce5d30b82c | 54 | |
| pinofal | 12:00ce5d30b82c | 55 | // Periodo di generazione campioni in output DeltaT = T/NumSample |
| pinofal | 14:d58d474fae35 | 56 | double fDeltaTClacsonSound; |
| pinofal | 14:d58d474fae35 | 57 | double fDeltaTEngineSound; |
| pinofal | 12:00ce5d30b82c | 58 | |
| pinofal | 14:d58d474fae35 | 59 | // amplificazione per i suoni da generare con l'ADC |
| pinofal | 14:d58d474fae35 | 60 | double fAmpEngineSound; // rumore di Engine |
| pinofal | 14:d58d474fae35 | 61 | double fAmpClacsonSound; // rumore di Clacson |
| pinofal | 14:d58d474fae35 | 62 | double fAmpShearSound; // rumore di Shear |
| pinofal | 14:d58d474fae35 | 63 | |
| pinofal | 14:d58d474fae35 | 64 | // frequenza segnale audio da generare per clacson e motore |
| pinofal | 14:d58d474fae35 | 65 | double fFreqClacsonSound; |
| pinofal | 14:d58d474fae35 | 66 | double fFreqEngineSound; |
| pinofal | 12:00ce5d30b82c | 67 | |
| pinofal | 14:d58d474fae35 | 68 | // periodo della sinusoide audio da generare come suono del clacson |
| pinofal | 14:d58d474fae35 | 69 | double fPeriodClacsonSOund; |
| pinofal | 12:00ce5d30b82c | 70 | |
| pinofal | 14:d58d474fae35 | 71 | // numero di campioni di clacson già inviati in output sul DAC |
| pinofal | 14:d58d474fae35 | 72 | int nClacsonSampleCount; |
| pinofal | 14:d58d474fae35 | 73 | // indice dell'array di generazione campioni clacson |
| pinofal | 14:d58d474fae35 | 74 | int nClacsonSampleIndex; |
| pinofal | 12:00ce5d30b82c | 75 | |
| pinofal | 14:d58d474fae35 | 76 | // indice dell'Array di generazione suoni del motore |
| pinofal | 14:d58d474fae35 | 77 | volatile int nEngineSampleIndex; |
| pinofal | 14:d58d474fae35 | 78 | |
| pinofal | 14:d58d474fae35 | 79 | // Flag che decide se generare oppure no il suono del motore. '1'=non generare il suono del motore, '0'=genera il suono del motore |
| pinofal | 14:d58d474fae35 | 80 | int bEngineSoundStop; |
| pinofal | 14:d58d474fae35 | 81 | |
| pinofal | 12:00ce5d30b82c | 82 | |
| pinofal | 12:00ce5d30b82c | 83 | |
| pinofal | 12:00ce5d30b82c | 84 | // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni |
| pinofal | 12:00ce5d30b82c | 85 | double fAvgLight; |
| pinofal | 12:00ce5d30b82c | 86 | |
| pinofal | 12:00ce5d30b82c | 87 | // valore numerico, di tensione e di luce letto dall'ADC |
| pinofal | 12:00ce5d30b82c | 88 | volatile unsigned short usReadADC; |
| pinofal | 12:00ce5d30b82c | 89 | volatile float fReadVoltage; |
| pinofal | 12:00ce5d30b82c | 90 | |
| pinofal | 12:00ce5d30b82c | 91 | // valore di luminosità letto dall'ADC |
| pinofal | 12:00ce5d30b82c | 92 | volatile float fLight; |
| pinofal | 12:00ce5d30b82c | 93 | |
| pinofal | 14:d58d474fae35 | 94 | // posizione del Cofano '0' = chiuso, '1'=aperto. Inizialmente DEVE essere chiuso (cioè '0') |
| pinofal | 14:d58d474fae35 | 95 | int nPosizioneCofano=0; |
| pinofal | 14:d58d474fae35 | 96 | |
| pinofal | 14:d58d474fae35 | 97 | |
| pinofal | 14:d58d474fae35 | 98 | // indice per il conteggio dei campioni di luce acquisiti dal fotoresistore |
| pinofal | 14:d58d474fae35 | 99 | int nLightSampleIndex; |
| pinofal | 14:d58d474fae35 | 100 | |
| pinofal | 14:d58d474fae35 | 101 | // timer per il calcolo della velocità |
| pinofal | 14:d58d474fae35 | 102 | Timer TimerHall; |
| pinofal | 14:d58d474fae35 | 103 | |
| pinofal | 14:d58d474fae35 | 104 | |
| pinofal | 14:d58d474fae35 | 105 | |
| pinofal | 14:d58d474fae35 | 106 | // variabile che conta il numero di fronti si salita del segnale encoder |
| pinofal | 14:d58d474fae35 | 107 | volatile int nCountRiseEdge; |
| pinofal | 14:d58d474fae35 | 108 | |
| pinofal | 14:d58d474fae35 | 109 | // pin di pilotaggio Motore DC |
| pinofal | 14:d58d474fae35 | 110 | DigitalOut OutMotorA (PB_0); |
| pinofal | 14:d58d474fae35 | 111 | DigitalOut OutMotorB (PC_1); |
| pinofal | 14:d58d474fae35 | 112 | |
| pinofal | 12:00ce5d30b82c | 113 | // Output Digitali usati per i LED |
| pinofal | 12:00ce5d30b82c | 114 | DigitalOut LedWAD (PC_2); |
| pinofal | 12:00ce5d30b82c | 115 | DigitalOut LedWAS (PC_3); |
| pinofal | 13:459e008582b3 | 116 | DigitalOut LedWPD (PH_0); |
| pinofal | 12:00ce5d30b82c | 117 | DigitalOut LedWPS (PA_0) ; |
| pinofal | 14:d58d474fae35 | 118 | DigitalOut LedYAD (PC_9); |
| pinofal | 14:d58d474fae35 | 119 | DigitalOut LedYAS (PC_8); |
| pinofal | 13:459e008582b3 | 120 | DigitalOut LedRPD (PA_13); |
| pinofal | 13:459e008582b3 | 121 | DigitalOut LedRPS (PA_14) ; |
| pinofal | 13:459e008582b3 | 122 | |
| pinofal | 13:459e008582b3 | 123 | |
| pinofal | 12:00ce5d30b82c | 124 | // Input/Output Digitali usati per interfaccia RPI |
| pinofal | 14:d58d474fae35 | 125 | DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 |
| pinofal | 14:d58d474fae35 | 126 | DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 |
| pinofal | 14:d58d474fae35 | 127 | DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16 |
| pinofal | 14:d58d474fae35 | 128 | DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13 |
| pinofal | 14:d58d474fae35 | 129 | DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25 |
| pinofal | 14:d58d474fae35 | 130 | DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12 |
| pinofal | 12:00ce5d30b82c | 131 | |
| pinofal | 14:d58d474fae35 | 132 | // Input e Output per i sensori e attuatori |
| pinofal | 14:d58d474fae35 | 133 | AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono |
| pinofal | 14:d58d474fae35 | 134 | AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità |
| pinofal | 14:d58d474fae35 | 135 | DigitalInOut InOutProxSensor (PC_0, PIN_OUTPUT, PullDown, 0); // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni |
| pinofal | 14:d58d474fae35 | 136 | InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento |
| pinofal | 14:d58d474fae35 | 137 | InterruptIn InEncoderB(PC_7); // Secondo Pin di input dall'encoder ottico collegato al motore. predisposizione per usi futuri |
| pinofal | 12:00ce5d30b82c | 138 | |
| pinofal | 14:d58d474fae35 | 139 | // Input/Output utilizzati da funzioni default su scheda NUCLEO |
| pinofal | 14:d58d474fae35 | 140 | DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5 |
| pinofal | 14:d58d474fae35 | 141 | Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 |
| pinofal | 14:d58d474fae35 | 142 | DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 |
| pinofal | 14:d58d474fae35 | 143 | |
| pinofal | 14:d58d474fae35 | 144 | // input di diagnostica |
| pinofal | 14:d58d474fae35 | 145 | DigitalIn InDiag1(PA_15,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN7 pin17-pin19 |
| pinofal | 14:d58d474fae35 | 146 | //DigitalIn InDiag2(PB_11,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 |
| pinofal | 12:00ce5d30b82c | 147 | |
| pinofal | 12:00ce5d30b82c | 148 | |
| pinofal | 12:00ce5d30b82c | 149 | //**************************** |
| pinofal | 12:00ce5d30b82c | 150 | // Create the sinewave buffer |
| pinofal | 12:00ce5d30b82c | 151 | //**************************** |
| pinofal | 12:00ce5d30b82c | 152 | void CalculateSinewave(int nOffset, int nAmplitude, double fPhase) |
| pinofal | 12:00ce5d30b82c | 153 | { |
| pinofal | 12:00ce5d30b82c | 154 | // variabile contenente l'angolo in radianti |
| pinofal | 12:00ce5d30b82c | 155 | double fRads; |
| pinofal | 12:00ce5d30b82c | 156 | // indici per i cicli |
| pinofal | 12:00ce5d30b82c | 157 | int nIndex; |
| pinofal | 12:00ce5d30b82c | 158 | // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE |
| pinofal | 12:00ce5d30b82c | 159 | double fDeltaF; |
| pinofal | 12:00ce5d30b82c | 160 | // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF |
| pinofal | 12:00ce5d30b82c | 161 | double fAngle; |
| pinofal | 12:00ce5d30b82c | 162 | |
| pinofal | 14:d58d474fae35 | 163 | fDeltaF = 360.0/CLACSONSAMPLENUM; |
| pinofal | 14:d58d474fae35 | 164 | for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) |
| pinofal | 12:00ce5d30b82c | 165 | { |
| pinofal | 12:00ce5d30b82c | 166 | fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide |
| pinofal | 12:00ce5d30b82c | 167 | fRads = (PI * fAngle)/180.0; // Convert degree in radian |
| pinofal | 12:00ce5d30b82c | 168 | //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET; |
| pinofal | 14:d58d474fae35 | 169 | usaClacson[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset; |
| pinofal | 12:00ce5d30b82c | 170 | } |
| pinofal | 12:00ce5d30b82c | 171 | } |
| pinofal | 12:00ce5d30b82c | 172 | |
| pinofal | 12:00ce5d30b82c | 173 | /********************************************************/ |
| pinofal | 12:00ce5d30b82c | 174 | /* Funzione avviata all'inizio come saluto e Benvenuto */ |
| pinofal | 12:00ce5d30b82c | 175 | /********************************************************/ |
| pinofal | 12:00ce5d30b82c | 176 | void WelcomeMessage() |
| pinofal | 12:00ce5d30b82c | 177 | { |
| pinofal | 14:d58d474fae35 | 178 | // indice per i cicli interni alla funzione |
| pinofal | 14:d58d474fae35 | 179 | int nIndex; |
| pinofal | 14:d58d474fae35 | 180 | |
| pinofal | 14:d58d474fae35 | 181 | // indice per l'array di welcome message |
| pinofal | 14:d58d474fae35 | 182 | int nWelcomeMsgIndex; |
| pinofal | 14:d58d474fae35 | 183 | // parametri per generare il messaggio di welcome |
| pinofal | 14:d58d474fae35 | 184 | double fAmpWelcomeSound; |
| pinofal | 14:d58d474fae35 | 185 | double fFreqWelcomeSound; |
| pinofal | 14:d58d474fae35 | 186 | double fDeltaTWelcomeSound; |
| pinofal | 14:d58d474fae35 | 187 | |
| pinofal | 14:d58d474fae35 | 188 | //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 189 | // accendi tutte le luci |
| pinofal | 14:d58d474fae35 | 190 | LedWAD = 1; |
| pinofal | 14:d58d474fae35 | 191 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 192 | LedWAS = 1; |
| pinofal | 14:d58d474fae35 | 193 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 194 | LedWPD = 1; |
| pinofal | 14:d58d474fae35 | 195 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 196 | LedWPS = 1; |
| pinofal | 14:d58d474fae35 | 197 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 198 | LedYAD = 1; |
| pinofal | 14:d58d474fae35 | 199 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 200 | LedYAS = 1; |
| pinofal | 14:d58d474fae35 | 201 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 202 | LedRPD = 1; |
| pinofal | 14:d58d474fae35 | 203 | wait_ms(300); |
| pinofal | 14:d58d474fae35 | 204 | LedRPS = 1; |
| pinofal | 14:d58d474fae35 | 205 | //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 206 | |
| pinofal | 14:d58d474fae35 | 207 | //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 208 | fAmpWelcomeSound = 1.0; // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] |
| pinofal | 14:d58d474fae35 | 209 | fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor |
| pinofal | 14:d58d474fae35 | 210 | fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 211 | |
| pinofal | 14:d58d474fae35 | 212 | |
| pinofal | 14:d58d474fae35 | 213 | for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++) |
| pinofal | 14:d58d474fae35 | 214 | { |
| pinofal | 14:d58d474fae35 | 215 | // mette in output un campione della forma d'onda del welcome message moltiplicato per l'amplificazione fAmp |
| pinofal | 14:d58d474fae35 | 216 | OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound); |
| pinofal | 14:d58d474fae35 | 217 | |
| pinofal | 14:d58d474fae35 | 218 | // tra un campione e l'altro attendi un periodo pari al periodo di campionamento |
| pinofal | 14:d58d474fae35 | 219 | //wait(fDeltaTWelcomeSound); |
| pinofal | 14:d58d474fae35 | 220 | wait_us(50); |
| pinofal | 14:d58d474fae35 | 221 | } |
| pinofal | 14:d58d474fae35 | 222 | //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 223 | |
| pinofal | 14:d58d474fae35 | 224 | //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 225 | // spegni le Luci in sequenza |
| pinofal | 14:d58d474fae35 | 226 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 227 | { |
| pinofal | 14:d58d474fae35 | 228 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 229 | LedWAD = 1; |
| pinofal | 14:d58d474fae35 | 230 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 231 | LedWAD = 0; |
| pinofal | 14:d58d474fae35 | 232 | } |
| pinofal | 14:d58d474fae35 | 233 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 234 | { |
| pinofal | 14:d58d474fae35 | 235 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 236 | LedWAS = 1; |
| pinofal | 14:d58d474fae35 | 237 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 238 | LedWAS = 0; |
| pinofal | 14:d58d474fae35 | 239 | } |
| pinofal | 14:d58d474fae35 | 240 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 241 | { |
| pinofal | 14:d58d474fae35 | 242 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 243 | LedWPD = 1; |
| pinofal | 14:d58d474fae35 | 244 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 245 | LedWPD = 0; |
| pinofal | 14:d58d474fae35 | 246 | } |
| pinofal | 14:d58d474fae35 | 247 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 248 | { |
| pinofal | 14:d58d474fae35 | 249 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 250 | LedWPS = 1; |
| pinofal | 14:d58d474fae35 | 251 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 252 | LedWPS = 0; |
| pinofal | 14:d58d474fae35 | 253 | } |
| pinofal | 14:d58d474fae35 | 254 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 255 | { |
| pinofal | 14:d58d474fae35 | 256 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 257 | LedYAD = 1; |
| pinofal | 14:d58d474fae35 | 258 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 259 | LedYAD =0; |
| pinofal | 14:d58d474fae35 | 260 | } |
| pinofal | 14:d58d474fae35 | 261 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 262 | { |
| pinofal | 14:d58d474fae35 | 263 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 264 | LedYAS = 1; |
| pinofal | 14:d58d474fae35 | 265 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 266 | LedYAS = 0; |
| pinofal | 14:d58d474fae35 | 267 | } |
| pinofal | 14:d58d474fae35 | 268 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 269 | { |
| pinofal | 14:d58d474fae35 | 270 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 271 | LedRPD = 1; |
| pinofal | 14:d58d474fae35 | 272 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 273 | LedRPD = 0; |
| pinofal | 14:d58d474fae35 | 274 | } |
| pinofal | 14:d58d474fae35 | 275 | for(nIndex=0; nIndex<3; nIndex++) |
| pinofal | 14:d58d474fae35 | 276 | { |
| pinofal | 14:d58d474fae35 | 277 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 278 | LedRPS = 1; |
| pinofal | 14:d58d474fae35 | 279 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 280 | LedRPS = 0; |
| pinofal | 14:d58d474fae35 | 281 | } |
| pinofal | 14:d58d474fae35 | 282 | //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 283 | |
| pinofal | 12:00ce5d30b82c | 284 | } |
| pinofal | 12:00ce5d30b82c | 285 | |
| pinofal | 14:d58d474fae35 | 286 | |
| pinofal | 14:d58d474fae35 | 287 | /***********************************************************************/ |
| pinofal | 14:d58d474fae35 | 288 | /* Genera il suono di una motosega. */ |
| pinofal | 14:d58d474fae35 | 289 | /* Attivo quando arriva il comando di spostamento Cesoie da Raspberry */ |
| pinofal | 14:d58d474fae35 | 290 | /***********************************************************************/ |
| pinofal | 14:d58d474fae35 | 291 | void ShearSoundGeneration() |
| pinofal | 12:00ce5d30b82c | 292 | { |
| pinofal | 14:d58d474fae35 | 293 | // indice per l'array di suono Shear |
| pinofal | 14:d58d474fae35 | 294 | int nShearSoundIndex; |
| pinofal | 14:d58d474fae35 | 295 | // parametri per generare il messaggio di shear |
| pinofal | 14:d58d474fae35 | 296 | double fAmpShearSound; |
| pinofal | 14:d58d474fae35 | 297 | double fFreqShearSound; |
| pinofal | 14:d58d474fae35 | 298 | double fDeltaTShearSound; |
| pinofal | 14:d58d474fae35 | 299 | |
| pinofal | 14:d58d474fae35 | 300 | //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 301 | fAmpShearSound = 1.0; // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] |
| pinofal | 14:d58d474fae35 | 302 | fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor |
| pinofal | 14:d58d474fae35 | 303 | fDeltaTShearSound = (1.0/fFreqShearSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 304 | |
| pinofal | 14:d58d474fae35 | 305 | |
| pinofal | 14:d58d474fae35 | 306 | for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++) |
| pinofal | 14:d58d474fae35 | 307 | { |
| pinofal | 14:d58d474fae35 | 308 | // mette in output un campione della forma d'onda del suono di Shear, moltiplicato per l'amplificazione fAmp |
| pinofal | 14:d58d474fae35 | 309 | OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound); |
| pinofal | 14:d58d474fae35 | 310 | |
| pinofal | 14:d58d474fae35 | 311 | // tra un campione e l'altro attendi un periodo pari al periodo di campionamento |
| pinofal | 14:d58d474fae35 | 312 | wait(fDeltaTShearSound); |
| pinofal | 14:d58d474fae35 | 313 | } |
| pinofal | 14:d58d474fae35 | 314 | //++++++++++++ FINE generazione suono di motosega +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 315 | |
| pinofal | 14:d58d474fae35 | 316 | |
| pinofal | 14:d58d474fae35 | 317 | |
| pinofal | 14:d58d474fae35 | 318 | } |
| pinofal | 14:d58d474fae35 | 319 | /***********************************************************************/ |
| pinofal | 14:d58d474fae35 | 320 | /* generazione suoni con i sample da file di campioni in SoundSample.h */ |
| pinofal | 14:d58d474fae35 | 321 | /***********************************************************************/ |
| pinofal | 14:d58d474fae35 | 322 | void SampleOut() |
| pinofal | 14:d58d474fae35 | 323 | { |
| pinofal | 14:d58d474fae35 | 324 | // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni |
| pinofal | 14:d58d474fae35 | 325 | if(bEngineSoundStop == 0) |
| pinofal | 14:d58d474fae35 | 326 | { |
| pinofal | 14:d58d474fae35 | 327 | // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp |
| pinofal | 14:d58d474fae35 | 328 | OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound); |
| pinofal | 14:d58d474fae35 | 329 | // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h |
| pinofal | 14:d58d474fae35 | 330 | nEngineSampleIndex++; |
| pinofal | 14:d58d474fae35 | 331 | if(nEngineSampleIndex >= nSampleNum) |
| pinofal | 14:d58d474fae35 | 332 | nEngineSampleIndex=0; |
| pinofal | 14:d58d474fae35 | 333 | } |
| pinofal | 12:00ce5d30b82c | 334 | } |
| pinofal | 12:00ce5d30b82c | 335 | |
| pinofal | 14:d58d474fae35 | 336 | |
| pinofal | 14:d58d474fae35 | 337 | /**************************************************************************************/ |
| pinofal | 14:d58d474fae35 | 338 | /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */ |
| pinofal | 14:d58d474fae35 | 339 | /**************************************************************************************/ |
| pinofal | 14:d58d474fae35 | 340 | void riseEncoderIRQ() |
| pinofal | 12:00ce5d30b82c | 341 | { |
| pinofal | 14:d58d474fae35 | 342 | nCountRiseEdge++; |
| pinofal | 14:d58d474fae35 | 343 | } |
| pinofal | 14:d58d474fae35 | 344 | |
| pinofal | 12:00ce5d30b82c | 345 | /********/ |
| pinofal | 12:00ce5d30b82c | 346 | /* Main */ |
| pinofal | 12:00ce5d30b82c | 347 | /********/ |
| pinofal | 12:00ce5d30b82c | 348 | int main() |
| pinofal | 12:00ce5d30b82c | 349 | { |
| pinofal | 12:00ce5d30b82c | 350 | // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto |
| pinofal | 12:00ce5d30b82c | 351 | pc.baud(921600); //921600 bps |
| pinofal | 12:00ce5d30b82c | 352 | |
| pinofal | 14:d58d474fae35 | 353 | // definisci il mode del segnale digitale di EncoderA |
| pinofal | 14:d58d474fae35 | 354 | InEncoderA.mode(PullUp); |
| pinofal | 12:00ce5d30b82c | 355 | |
| pinofal | 14:d58d474fae35 | 356 | // Associa routine di Interrup all'evento fronte di salita del segnale di encoder |
| pinofal | 14:d58d474fae35 | 357 | InEncoderA.rise(&riseEncoderIRQ); |
| pinofal | 14:d58d474fae35 | 358 | |
| pinofal | 14:d58d474fae35 | 359 | // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove |
| pinofal | 14:d58d474fae35 | 360 | InEncoderA.enable_irq(); |
| pinofal | 12:00ce5d30b82c | 361 | |
| pinofal | 14:d58d474fae35 | 362 | // avvia routine di saluto di benvenuto |
| pinofal | 14:d58d474fae35 | 363 | WelcomeMessage(); |
| pinofal | 14:d58d474fae35 | 364 | |
| pinofal | 14:d58d474fae35 | 365 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 366 | //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 367 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 12:00ce5d30b82c | 368 | |
| pinofal | 12:00ce5d30b82c | 369 | //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 370 | fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare |
| pinofal | 14:d58d474fae35 | 371 | fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare |
| pinofal | 14:d58d474fae35 | 372 | fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata |
| pinofal | 14:d58d474fae35 | 373 | CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali |
| pinofal | 12:00ce5d30b82c | 374 | //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 375 | |
| pinofal | 14:d58d474fae35 | 376 | //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++ |
| pinofal | 14:d58d474fae35 | 377 | fAmpEngineSound = 1.0; // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] |
| pinofal | 14:d58d474fae35 | 378 | fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor |
| pinofal | 14:d58d474fae35 | 379 | fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 380 | nEngineSampleIndex =0; // Avvia indice di generazione suono motore |
| pinofal | 14:d58d474fae35 | 381 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 382 | //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++ |
| pinofal | 14:d58d474fae35 | 383 | |
| pinofal | 14:d58d474fae35 | 384 | //inizializza variabili |
| pinofal | 14:d58d474fae35 | 385 | nEngineSampleIndex =0; // avvia l'indice di generazione suoni |
| pinofal | 14:d58d474fae35 | 386 | nCountRiseEdge=0; // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA |
| pinofal | 14:d58d474fae35 | 387 | bEngineSoundStop =0; // inizialmente il suono del motore è generato |
| pinofal | 14:d58d474fae35 | 388 | nPosizioneCofano=0; // inizializza la posizione del cofano chiuso |
| pinofal | 12:00ce5d30b82c | 389 | while(true) |
| pinofal | 12:00ce5d30b82c | 390 | { |
| pinofal | 14:d58d474fae35 | 391 | //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 392 | // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo |
| pinofal | 14:d58d474fae35 | 393 | if(nCountRiseEdge != 0) |
| pinofal | 14:d58d474fae35 | 394 | //if(InDiag1==1) |
| pinofal | 14:d58d474fae35 | 395 | { |
| pinofal | 14:d58d474fae35 | 396 | // sono stati contati impulsi di encoder, il robot si sta muovendo |
| pinofal | 14:d58d474fae35 | 397 | fDeltaTEngineSound = (0.5/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 398 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 399 | } |
| pinofal | 14:d58d474fae35 | 400 | else |
| pinofal | 14:d58d474fae35 | 401 | { |
| pinofal | 14:d58d474fae35 | 402 | // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo |
| pinofal | 14:d58d474fae35 | 403 | fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 404 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 405 | |
| pinofal | 14:d58d474fae35 | 406 | } |
| pinofal | 14:d58d474fae35 | 407 | nCountRiseEdge=0; |
| pinofal | 14:d58d474fae35 | 408 | //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++ |
| pinofal | 12:00ce5d30b82c | 409 | |
| pinofal | 14:d58d474fae35 | 410 | //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++ |
| pinofal | 14:d58d474fae35 | 411 | // inizializza il valore medio della Luminosità |
| pinofal | 14:d58d474fae35 | 412 | fAvgLight=0.0; |
| pinofal | 14:d58d474fae35 | 413 | for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++) |
| pinofal | 14:d58d474fae35 | 414 | { |
| pinofal | 14:d58d474fae35 | 415 | // acquisisce dato da ADC |
| pinofal | 14:d58d474fae35 | 416 | usReadADC = InWaveLight.read_u16(); |
| pinofal | 14:d58d474fae35 | 417 | fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC |
| pinofal | 14:d58d474fae35 | 418 | //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt |
| pinofal | 14:d58d474fae35 | 419 | fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC |
| pinofal | 14:d58d474fae35 | 420 | fAvgLight+=fLight; |
| pinofal | 14:d58d474fae35 | 421 | } |
| pinofal | 14:d58d474fae35 | 422 | // calcola valore medio su NUMSAMPLE acquisizioni |
| pinofal | 14:d58d474fae35 | 423 | fAvgLight/= NUMLIGHTSAMPLE; |
| pinofal | 14:d58d474fae35 | 424 | |
| pinofal | 14:d58d474fae35 | 425 | // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia |
| pinofal | 14:d58d474fae35 | 426 | if(fAvgLight < SOGLIALUCIMIN) |
| pinofal | 14:d58d474fae35 | 427 | { |
| pinofal | 14:d58d474fae35 | 428 | // Accendi LED Bianchi |
| pinofal | 14:d58d474fae35 | 429 | //led2 = 1; |
| pinofal | 14:d58d474fae35 | 430 | LedWAD = 1; |
| pinofal | 14:d58d474fae35 | 431 | LedWAS = 1; |
| pinofal | 14:d58d474fae35 | 432 | LedWPD = 1; |
| pinofal | 14:d58d474fae35 | 433 | LedWPS = 1; |
| pinofal | 14:d58d474fae35 | 434 | } |
| pinofal | 14:d58d474fae35 | 435 | else |
| pinofal | 14:d58d474fae35 | 436 | { |
| pinofal | 14:d58d474fae35 | 437 | if(fAvgLight > SOGLIALUCIMAX) |
| pinofal | 14:d58d474fae35 | 438 | { |
| pinofal | 14:d58d474fae35 | 439 | // Spegni LED Bianchi |
| pinofal | 14:d58d474fae35 | 440 | //led2 = 0; |
| pinofal | 14:d58d474fae35 | 441 | LedWAD = 0; |
| pinofal | 14:d58d474fae35 | 442 | LedWAS = 0; |
| pinofal | 14:d58d474fae35 | 443 | LedWPD = 0; |
| pinofal | 14:d58d474fae35 | 444 | LedWPS = 0; |
| pinofal | 14:d58d474fae35 | 445 | } |
| pinofal | 14:d58d474fae35 | 446 | } |
| pinofal | 14:d58d474fae35 | 447 | |
| pinofal | 14:d58d474fae35 | 448 | // invia il dato al PC |
| pinofal | 14:d58d474fae35 | 449 | //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight); |
| pinofal | 14:d58d474fae35 | 450 | //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++ |
| pinofal | 14:d58d474fae35 | 451 | //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++ |
| pinofal | 14:d58d474fae35 | 452 | //inizializza misura di distanza |
| pinofal | 14:d58d474fae35 | 453 | fDistance=0.0; |
| pinofal | 14:d58d474fae35 | 454 | // Fissa come Output il pin InOutProxSensor |
| pinofal | 14:d58d474fae35 | 455 | InOutProxSensor.output(); |
| pinofal | 14:d58d474fae35 | 456 | // Poni 'L' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 457 | InOutProxSensor.write(0); |
| pinofal | 14:d58d474fae35 | 458 | wait_us(5); |
| pinofal | 14:d58d474fae35 | 459 | // Poni 'H' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 460 | InOutProxSensor.write(1); |
| pinofal | 14:d58d474fae35 | 461 | wait_us(10); |
| pinofal | 14:d58d474fae35 | 462 | // Poni 'L' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 463 | InOutProxSensor.write(0); |
| pinofal | 14:d58d474fae35 | 464 | // Attendi assestamento e Fissa come Input il pin InOutProxSensor |
| pinofal | 14:d58d474fae35 | 465 | wait_us(5); |
| pinofal | 14:d58d474fae35 | 466 | InOutProxSensor.input(); |
| pinofal | 14:d58d474fae35 | 467 | InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0' |
| pinofal | 14:d58d474fae35 | 468 | |
| pinofal | 14:d58d474fae35 | 469 | // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore |
| pinofal | 14:d58d474fae35 | 470 | TimerProxSensor.start(); |
| pinofal | 14:d58d474fae35 | 471 | nTimerStart = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 472 | nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart); |
| pinofal | 14:d58d474fae35 | 473 | while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR)) |
| pinofal | 14:d58d474fae35 | 474 | { |
| pinofal | 14:d58d474fae35 | 475 | nTimerCurrent = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 476 | nTimerTillNow=nTimerCurrent-nTimerStart; |
| pinofal | 14:d58d474fae35 | 477 | led2=1; // se rimane nel while il LED rimane acceso |
| pinofal | 14:d58d474fae35 | 478 | pc.printf("sono qui 2 \r\n"); |
| pinofal | 14:d58d474fae35 | 479 | } |
| pinofal | 14:d58d474fae35 | 480 | TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità |
| pinofal | 14:d58d474fae35 | 481 | pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow); |
| pinofal | 14:d58d474fae35 | 482 | // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo |
| pinofal | 14:d58d474fae35 | 483 | if(nTimerTillNow < TIMEOUTPROXSENSOR) |
| pinofal | 14:d58d474fae35 | 484 | { |
| pinofal | 14:d58d474fae35 | 485 | // riattiva il timer per misurare la distanza dell'ostacolo |
| pinofal | 14:d58d474fae35 | 486 | TimerProxSensor.start(); |
| pinofal | 14:d58d474fae35 | 487 | nTimerStart = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 488 | while(InOutProxSensor == 1) |
| pinofal | 14:d58d474fae35 | 489 | { |
| pinofal | 14:d58d474fae35 | 490 | led2=1; // se rimane nel while il LED rimane acceso |
| pinofal | 14:d58d474fae35 | 491 | } |
| pinofal | 14:d58d474fae35 | 492 | TimerProxSensor.stop(); |
| pinofal | 14:d58d474fae35 | 493 | nTimerStop = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 494 | |
| pinofal | 14:d58d474fae35 | 495 | pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow); |
| pinofal | 14:d58d474fae35 | 496 | |
| pinofal | 14:d58d474fae35 | 497 | // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us] |
| pinofal | 14:d58d474fae35 | 498 | // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore |
| pinofal | 14:d58d474fae35 | 499 | // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us] |
| pinofal | 14:d58d474fae35 | 500 | fDistance = (nTimerStop-nTimerStart)/58.2; |
| pinofal | 14:d58d474fae35 | 501 | // invia il dato al PC |
| pinofal | 14:d58d474fae35 | 502 | pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance); |
| pinofal | 14:d58d474fae35 | 503 | } |
| pinofal | 14:d58d474fae35 | 504 | else |
| pinofal | 12:00ce5d30b82c | 505 | { |
| pinofal | 14:d58d474fae35 | 506 | // quando esce dai while bloccanti, il LED si spegne |
| pinofal | 14:d58d474fae35 | 507 | led2=0; |
| pinofal | 14:d58d474fae35 | 508 | pc.printf("\r\nTimeOut\r\n"); |
| pinofal | 12:00ce5d30b82c | 509 | } |
| pinofal | 14:d58d474fae35 | 510 | //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ |
| pinofal | 14:d58d474fae35 | 511 | //++++++++++++++ INIZIO Suona Clacson +++++++++ |
| pinofal | 14:d58d474fae35 | 512 | //escludi le misure oltre il max |
| pinofal | 14:d58d474fae35 | 513 | if((fDistance <= 50.0) && (fDistance >= 3)) |
| pinofal | 14:d58d474fae35 | 514 | //if(InDiag1 == 1) |
| pinofal | 12:00ce5d30b82c | 515 | { |
| pinofal | 14:d58d474fae35 | 516 | // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima |
| pinofal | 14:d58d474fae35 | 517 | if(fDistance < 22) |
| pinofal | 14:d58d474fae35 | 518 | { |
| pinofal | 14:d58d474fae35 | 519 | // blocca altri suoni quando genera suono del clacson |
| pinofal | 14:d58d474fae35 | 520 | bEngineSoundStop=1; |
| pinofal | 14:d58d474fae35 | 521 | // INIZIO generazione tono |
| pinofal | 14:d58d474fae35 | 522 | nClacsonSampleIndex=0; |
| pinofal | 14:d58d474fae35 | 523 | // Genera il suono del clacson |
| pinofal | 14:d58d474fae35 | 524 | for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++) |
| pinofal | 14:d58d474fae35 | 525 | { |
| pinofal | 14:d58d474fae35 | 526 | OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767 |
| pinofal | 14:d58d474fae35 | 527 | //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici |
| pinofal | 14:d58d474fae35 | 528 | wait(fDeltaTClacsonSound); |
| pinofal | 14:d58d474fae35 | 529 | // genera ciclicamente |
| pinofal | 14:d58d474fae35 | 530 | nClacsonSampleIndex++; |
| pinofal | 14:d58d474fae35 | 531 | if(nClacsonSampleIndex >= CLACSONSAMPLENUM) |
| pinofal | 14:d58d474fae35 | 532 | { |
| pinofal | 14:d58d474fae35 | 533 | nClacsonSampleIndex=0; |
| pinofal | 14:d58d474fae35 | 534 | } |
| pinofal | 14:d58d474fae35 | 535 | // a metà genera un wait per doppio clacson |
| pinofal | 14:d58d474fae35 | 536 | if(nClacsonSampleCount == 2000) |
| pinofal | 14:d58d474fae35 | 537 | { |
| pinofal | 14:d58d474fae35 | 538 | wait_ms(100); |
| pinofal | 14:d58d474fae35 | 539 | } |
| pinofal | 14:d58d474fae35 | 540 | |
| pinofal | 14:d58d474fae35 | 541 | } |
| pinofal | 14:d58d474fae35 | 542 | //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente |
| pinofal | 14:d58d474fae35 | 543 | OutWave.write_u16(0); |
| pinofal | 14:d58d474fae35 | 544 | |
| pinofal | 14:d58d474fae35 | 545 | // sblocca altri suoni dopo aver generato suono del clacson |
| pinofal | 14:d58d474fae35 | 546 | bEngineSoundStop=0; |
| pinofal | 14:d58d474fae35 | 547 | |
| pinofal | 14:d58d474fae35 | 548 | } // if(fDistance < soglia) suona clacson |
| pinofal | 14:d58d474fae35 | 549 | |
| pinofal | 14:d58d474fae35 | 550 | } // if( (fDistance < Max) && (fDistance > Min)) |
| pinofal | 14:d58d474fae35 | 551 | //++++++++++++++ FINE Suona Clacson +++++++++ |
| pinofal | 14:d58d474fae35 | 552 | |
| pinofal | 14:d58d474fae35 | 553 | |
| pinofal | 14:d58d474fae35 | 554 | |
| pinofal | 14:d58d474fae35 | 555 | //++++++++++++++ INIZIO pilotaggio motore cofano +++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 556 | if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0)) |
| pinofal | 14:d58d474fae35 | 557 | //if((myButton==1) && (nPosizioneCofano ==0)) |
| pinofal | 14:d58d474fae35 | 558 | { |
| pinofal | 14:d58d474fae35 | 559 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 560 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 561 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 562 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 563 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 564 | |
| pinofal | 14:d58d474fae35 | 565 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 566 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 567 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 568 | //pc.printf("Stop motore; OutA OutB = 01\r\n"); |
| pinofal | 14:d58d474fae35 | 569 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 570 | |
| pinofal | 14:d58d474fae35 | 571 | // Ruota Right |
| pinofal | 14:d58d474fae35 | 572 | OutMotorA=1; |
| pinofal | 14:d58d474fae35 | 573 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 574 | //pc.printf("Ruota Right; OutA OutB = 11\r\n"); |
| pinofal | 14:d58d474fae35 | 575 | wait_ms(710); |
| pinofal | 14:d58d474fae35 | 576 | |
| pinofal | 14:d58d474fae35 | 577 | // Ferma Motore |
| pinofal | 14:d58d474fae35 | 578 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 579 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 580 | //pc.printf("Stop Motore; OutA OutB = 01\r\n"); |
| pinofal | 14:d58d474fae35 | 581 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 582 | |
| pinofal | 14:d58d474fae35 | 583 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 584 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 585 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 586 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 587 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 588 | // cambia posizione del cofano. E' Stato Aperto |
| pinofal | 14:d58d474fae35 | 589 | nPosizioneCofano = 1; |
| pinofal | 14:d58d474fae35 | 590 | } |
| pinofal | 14:d58d474fae35 | 591 | // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore |
| pinofal | 14:d58d474fae35 | 592 | //if((myButton==0) && (nPosizioneCofano == 1)) |
| pinofal | 14:d58d474fae35 | 593 | if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1)) |
| pinofal | 14:d58d474fae35 | 594 | { |
| pinofal | 14:d58d474fae35 | 595 | //pc.printf("\r\nCofano aperto & comando di chiusura\r\n"); |
| pinofal | 14:d58d474fae35 | 596 | |
| pinofal | 14:d58d474fae35 | 597 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 598 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 599 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 600 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 601 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 602 | |
| pinofal | 14:d58d474fae35 | 603 | // Ruota Left |
| pinofal | 14:d58d474fae35 | 604 | OutMotorA=1; |
| pinofal | 14:d58d474fae35 | 605 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 606 | //pc.printf("Ruota Left; OutA OutB = 10\r\n"); |
| pinofal | 14:d58d474fae35 | 607 | wait_ms(730); |
| pinofal | 14:d58d474fae35 | 608 | |
| pinofal | 14:d58d474fae35 | 609 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 610 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 611 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 612 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 613 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 614 | |
| pinofal | 14:d58d474fae35 | 615 | // cambia posizione del cofano. E' Stato Chiuso |
| pinofal | 14:d58d474fae35 | 616 | nPosizioneCofano = 0; |
| pinofal | 14:d58d474fae35 | 617 | } |
| pinofal | 12:00ce5d30b82c | 618 | //++++++++++++++ FINE Pilotaggio Motore +++++++++++++ |
| pinofal | 14:d58d474fae35 | 619 | |
| pinofal | 14:d58d474fae35 | 620 | |
| pinofal | 14:d58d474fae35 | 621 | |
| pinofal | 12:00ce5d30b82c | 622 | //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++ |
| pinofal | 12:00ce5d30b82c | 623 | if(InLightSwitchRPI ==1) |
| pinofal | 12:00ce5d30b82c | 624 | { |
| pinofal | 12:00ce5d30b82c | 625 | // accendi i LED di abbellimento |
| pinofal | 13:459e008582b3 | 626 | //led2=1; |
| pinofal | 13:459e008582b3 | 627 | LedYAD = 1; |
| pinofal | 13:459e008582b3 | 628 | LedYAS = 1; |
| pinofal | 13:459e008582b3 | 629 | LedRPD = 1; |
| pinofal | 13:459e008582b3 | 630 | LedRPS = 1; |
| pinofal | 12:00ce5d30b82c | 631 | } |
| pinofal | 12:00ce5d30b82c | 632 | else |
| pinofal | 12:00ce5d30b82c | 633 | { |
| pinofal | 13:459e008582b3 | 634 | |
| pinofal | 12:00ce5d30b82c | 635 | // spegni i LED di abbellimento |
| pinofal | 13:459e008582b3 | 636 | //led2=0; |
| pinofal | 13:459e008582b3 | 637 | LedYAD = 0; |
| pinofal | 13:459e008582b3 | 638 | LedYAS = 0; |
| pinofal | 13:459e008582b3 | 639 | LedRPD = 0; |
| pinofal | 13:459e008582b3 | 640 | LedRPS = 0; |
| pinofal | 13:459e008582b3 | 641 | |
| pinofal | 12:00ce5d30b82c | 642 | } |
| pinofal | 14:d58d474fae35 | 643 | //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++ |
| pinofal | 14:d58d474fae35 | 644 | |
| pinofal | 14:d58d474fae35 | 645 | //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ |
| pinofal | 14:d58d474fae35 | 646 | if(InShearRPI == 1) |
| pinofal | 14:d58d474fae35 | 647 | { |
| pinofal | 14:d58d474fae35 | 648 | // funzione di generazione suono motosega |
| pinofal | 14:d58d474fae35 | 649 | bEngineSoundStop=1; // disattiva suono del motore |
| pinofal | 14:d58d474fae35 | 650 | ShearSoundGeneration(); |
| pinofal | 14:d58d474fae35 | 651 | bEngineSoundStop=0; // riattiva suono del motore |
| pinofal | 14:d58d474fae35 | 652 | } |
| pinofal | 14:d58d474fae35 | 653 | //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ |
| pinofal | 14:d58d474fae35 | 654 | } |
| pinofal | 14:d58d474fae35 | 655 | //+++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 656 | |
| pinofal | 14:d58d474fae35 | 657 | |
| pinofal | 14:d58d474fae35 | 658 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 659 | //+++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 660 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 661 | //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 662 | fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare |
| pinofal | 14:d58d474fae35 | 663 | fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare |
| pinofal | 14:d58d474fae35 | 664 | fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata |
| pinofal | 14:d58d474fae35 | 665 | CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali |
| pinofal | 14:d58d474fae35 | 666 | //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 667 | |
| pinofal | 13:459e008582b3 | 668 | |
| pinofal | 14:d58d474fae35 | 669 | //+++++++ INIZIO avvio ruomre del motore a frequenza da fermo +++++++++ |
| pinofal | 14:d58d474fae35 | 670 | fAmpEngineSound = 1.0; // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] |
| pinofal | 14:d58d474fae35 | 671 | fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor |
| pinofal | 14:d58d474fae35 | 672 | fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 673 | nEngineSampleIndex =0; // Avvia indice di generazione suono motore |
| pinofal | 14:d58d474fae35 | 674 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 675 | //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++ |
| pinofal | 14:d58d474fae35 | 676 | |
| pinofal | 14:d58d474fae35 | 677 | //inizializza variabili |
| pinofal | 14:d58d474fae35 | 678 | nEngineSampleIndex =0; // avvia l'indice di generazione suoni |
| pinofal | 14:d58d474fae35 | 679 | nCountRiseEdge=0; // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA |
| pinofal | 14:d58d474fae35 | 680 | bEngineSoundStop =0; // inizialmente il suono del motore è generato |
| pinofal | 14:d58d474fae35 | 681 | |
| pinofal | 14:d58d474fae35 | 682 | //++++ INIZIO Ciclo Principale ++++ |
| pinofal | 14:d58d474fae35 | 683 | while (true) |
| pinofal | 14:d58d474fae35 | 684 | { |
| pinofal | 14:d58d474fae35 | 685 | //++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 686 | // abilita l'interrupt su fronte di salita del segnale di encoder |
| pinofal | 14:d58d474fae35 | 687 | nCountRiseEdge=0; |
| pinofal | 14:d58d474fae35 | 688 | InEncoderA.enable_irq(); |
| pinofal | 14:d58d474fae35 | 689 | |
| pinofal | 14:d58d474fae35 | 690 | // conta il numero di impulsi del segnale di encoder che si verificano in un timer pari a 500ms |
| pinofal | 14:d58d474fae35 | 691 | TimerHall.start(); |
| pinofal | 14:d58d474fae35 | 692 | nTimerStart=TimerHall.read_ms(); |
| pinofal | 14:d58d474fae35 | 693 | |
| pinofal | 14:d58d474fae35 | 694 | // per 500ms verifica se ci sono impulsi sull'encoder |
| pinofal | 14:d58d474fae35 | 695 | while( (nTimerCurrent-nTimerStart) < 300) // attende il passare di 300ms |
| pinofal | 14:d58d474fae35 | 696 | { |
| pinofal | 14:d58d474fae35 | 697 | nTimerCurrent=TimerHall.read_ms(); |
| pinofal | 14:d58d474fae35 | 698 | // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart)); |
| pinofal | 14:d58d474fae35 | 699 | } |
| pinofal | 14:d58d474fae35 | 700 | TimerHall.stop(); |
| pinofal | 14:d58d474fae35 | 701 | InEncoderA.disable_irq(); |
| pinofal | 14:d58d474fae35 | 702 | //++++++++++ FINE calcola spostamento con encoder sul motore +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 703 | |
| pinofal | 14:d58d474fae35 | 704 | //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 705 | // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo |
| pinofal | 14:d58d474fae35 | 706 | if(nCountRiseEdge != 0) |
| pinofal | 14:d58d474fae35 | 707 | //if(InDiag1==1) |
| pinofal | 14:d58d474fae35 | 708 | { |
| pinofal | 14:d58d474fae35 | 709 | // sono stati contati impulsi di encoder, il robot si sta muovendo |
| pinofal | 14:d58d474fae35 | 710 | fDeltaTEngineSound = (0.5/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 711 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 712 | } |
| pinofal | 14:d58d474fae35 | 713 | else |
| pinofal | 14:d58d474fae35 | 714 | { |
| pinofal | 14:d58d474fae35 | 715 | // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo |
| pinofal | 14:d58d474fae35 | 716 | fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento |
| pinofal | 14:d58d474fae35 | 717 | SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione |
| pinofal | 14:d58d474fae35 | 718 | } |
| pinofal | 14:d58d474fae35 | 719 | //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++ |
| pinofal | 14:d58d474fae35 | 720 | |
| pinofal | 14:d58d474fae35 | 721 | //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++ |
| pinofal | 14:d58d474fae35 | 722 | // se arriva comando di apertura & il cofano è chiuso, muovi motore |
| pinofal | 14:d58d474fae35 | 723 | if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0)) |
| pinofal | 14:d58d474fae35 | 724 | { |
| pinofal | 14:d58d474fae35 | 725 | //pc.printf("\r\ncofano chiuso & comando di apertura\r\n"); |
| pinofal | 14:d58d474fae35 | 726 | |
| pinofal | 14:d58d474fae35 | 727 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 728 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 729 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 730 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 731 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 732 | |
| pinofal | 14:d58d474fae35 | 733 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 734 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 735 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 736 | //pc.printf("Stop motore; OutA OutB = 01\r\n"); |
| pinofal | 14:d58d474fae35 | 737 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 738 | |
| pinofal | 14:d58d474fae35 | 739 | // Ruota Right |
| pinofal | 14:d58d474fae35 | 740 | OutMotorA=1; |
| pinofal | 14:d58d474fae35 | 741 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 742 | //pc.printf("Ruota Right; OutA OutB = 11\r\n"); |
| pinofal | 14:d58d474fae35 | 743 | wait_ms(710); |
| pinofal | 14:d58d474fae35 | 744 | |
| pinofal | 14:d58d474fae35 | 745 | // Ferma Motore |
| pinofal | 14:d58d474fae35 | 746 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 747 | OutMotorB=1; |
| pinofal | 14:d58d474fae35 | 748 | //pc.printf("Stop Motore; OutA OutB = 01\r\n"); |
| pinofal | 14:d58d474fae35 | 749 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 750 | |
| pinofal | 14:d58d474fae35 | 751 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 752 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 753 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 754 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 755 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 756 | // cambia posizione del cofano. E' Stato Aperto |
| pinofal | 14:d58d474fae35 | 757 | nPosizioneCofano = 1; |
| pinofal | 14:d58d474fae35 | 758 | } |
| pinofal | 14:d58d474fae35 | 759 | // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore |
| pinofal | 14:d58d474fae35 | 760 | if((myButton==0) && (nPosizioneCofano == 1)) |
| pinofal | 14:d58d474fae35 | 761 | { |
| pinofal | 14:d58d474fae35 | 762 | //pc.printf("\r\nCofano aperto & comando di chiusura\r\n"); |
| pinofal | 14:d58d474fae35 | 763 | |
| pinofal | 14:d58d474fae35 | 764 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 765 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 766 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 767 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 768 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 769 | |
| pinofal | 14:d58d474fae35 | 770 | // Ruota Left |
| pinofal | 14:d58d474fae35 | 771 | OutMotorA=1; |
| pinofal | 14:d58d474fae35 | 772 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 773 | //pc.printf("Ruota Left; OutA OutB = 10\r\n"); |
| pinofal | 14:d58d474fae35 | 774 | wait_ms(730); |
| pinofal | 14:d58d474fae35 | 775 | |
| pinofal | 14:d58d474fae35 | 776 | //Ferma motore |
| pinofal | 14:d58d474fae35 | 777 | OutMotorA=0; |
| pinofal | 14:d58d474fae35 | 778 | OutMotorB=0; |
| pinofal | 14:d58d474fae35 | 779 | //pc.printf("Stop motore; OutA OutB = 00\r\n"); |
| pinofal | 14:d58d474fae35 | 780 | wait_ms(10); |
| pinofal | 14:d58d474fae35 | 781 | |
| pinofal | 14:d58d474fae35 | 782 | // cambia posizione del cofano. E' Stato Chiuso |
| pinofal | 14:d58d474fae35 | 783 | nPosizioneCofano = 0; |
| pinofal | 14:d58d474fae35 | 784 | } |
| pinofal | 14:d58d474fae35 | 785 | //++++++++++++++ FINE Pilotaggio Motore +++++++++++++ |
| pinofal | 14:d58d474fae35 | 786 | //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++ |
| pinofal | 14:d58d474fae35 | 787 | if(InLightSwitchRPI ==1) |
| pinofal | 14:d58d474fae35 | 788 | { |
| pinofal | 14:d58d474fae35 | 789 | // accendi i LED di abbellimento |
| pinofal | 14:d58d474fae35 | 790 | //led2=1; |
| pinofal | 14:d58d474fae35 | 791 | LedYAD = 1; |
| pinofal | 14:d58d474fae35 | 792 | LedYAS = 1; |
| pinofal | 14:d58d474fae35 | 793 | LedRPD = 1; |
| pinofal | 14:d58d474fae35 | 794 | LedRPS = 1; |
| pinofal | 14:d58d474fae35 | 795 | } |
| pinofal | 14:d58d474fae35 | 796 | else |
| pinofal | 14:d58d474fae35 | 797 | { |
| pinofal | 14:d58d474fae35 | 798 | |
| pinofal | 14:d58d474fae35 | 799 | // spegni i LED di abbellimento |
| pinofal | 14:d58d474fae35 | 800 | //led2=0; |
| pinofal | 14:d58d474fae35 | 801 | LedYAD = 0; |
| pinofal | 14:d58d474fae35 | 802 | LedYAS = 0; |
| pinofal | 14:d58d474fae35 | 803 | LedRPD = 0; |
| pinofal | 14:d58d474fae35 | 804 | LedRPS = 0; |
| pinofal | 14:d58d474fae35 | 805 | |
| pinofal | 14:d58d474fae35 | 806 | } |
| pinofal | 14:d58d474fae35 | 807 | //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++ |
| pinofal | 14:d58d474fae35 | 808 | |
| pinofal | 14:d58d474fae35 | 809 | //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ |
| pinofal | 14:d58d474fae35 | 810 | if(InShearRPI == 1) |
| pinofal | 14:d58d474fae35 | 811 | { |
| pinofal | 14:d58d474fae35 | 812 | // funzione di generazione suono motosega |
| pinofal | 14:d58d474fae35 | 813 | bEngineSoundStop=1; // disattiva suono del motore |
| pinofal | 14:d58d474fae35 | 814 | ShearSoundGeneration(); |
| pinofal | 14:d58d474fae35 | 815 | bEngineSoundStop=0; // riattiva suono del motore |
| pinofal | 14:d58d474fae35 | 816 | } |
| pinofal | 14:d58d474fae35 | 817 | //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ |
| pinofal | 14:d58d474fae35 | 818 | |
| pinofal | 14:d58d474fae35 | 819 | |
| pinofal | 14:d58d474fae35 | 820 | //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++ |
| pinofal | 14:d58d474fae35 | 821 | // inizializza il valore medio della Luminosità |
| pinofal | 14:d58d474fae35 | 822 | fAvgLight=0.0; |
| pinofal | 14:d58d474fae35 | 823 | for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++) |
| pinofal | 14:d58d474fae35 | 824 | { |
| pinofal | 12:00ce5d30b82c | 825 | // acquisisce dato da ADC |
| pinofal | 12:00ce5d30b82c | 826 | usReadADC = InWaveLight.read_u16(); |
| pinofal | 12:00ce5d30b82c | 827 | fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC |
| pinofal | 12:00ce5d30b82c | 828 | //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt |
| pinofal | 12:00ce5d30b82c | 829 | fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC |
| pinofal | 12:00ce5d30b82c | 830 | fAvgLight+=fLight; |
| pinofal | 14:d58d474fae35 | 831 | } |
| pinofal | 14:d58d474fae35 | 832 | // calcola valore medio su NUMSAMPLE acquisizioni |
| pinofal | 14:d58d474fae35 | 833 | fAvgLight/= NUMLIGHTSAMPLE; |
| pinofal | 14:d58d474fae35 | 834 | |
| pinofal | 14:d58d474fae35 | 835 | // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia |
| pinofal | 14:d58d474fae35 | 836 | if(fAvgLight > SOGLIALUCIMAX) |
| pinofal | 14:d58d474fae35 | 837 | { |
| pinofal | 13:459e008582b3 | 838 | // Accendi LED Bianchi |
| pinofal | 13:459e008582b3 | 839 | //led2 = 1; |
| pinofal | 13:459e008582b3 | 840 | LedWAD = 1; |
| pinofal | 13:459e008582b3 | 841 | LedWAS = 1; |
| pinofal | 13:459e008582b3 | 842 | LedWPD = 1; |
| pinofal | 13:459e008582b3 | 843 | LedWPS = 1; |
| pinofal | 13:459e008582b3 | 844 | |
| pinofal | 14:d58d474fae35 | 845 | } |
| pinofal | 14:d58d474fae35 | 846 | else |
| pinofal | 14:d58d474fae35 | 847 | { |
| pinofal | 14:d58d474fae35 | 848 | if(fAvgLight < SOGLIALUCIMIN) |
| pinofal | 14:d58d474fae35 | 849 | { |
| pinofal | 14:d58d474fae35 | 850 | |
| pinofal | 14:d58d474fae35 | 851 | // Spegni LED Bianchi |
| pinofal | 14:d58d474fae35 | 852 | //led2 = 0; |
| pinofal | 14:d58d474fae35 | 853 | LedWAD = 0; |
| pinofal | 14:d58d474fae35 | 854 | LedWAS = 0; |
| pinofal | 14:d58d474fae35 | 855 | LedWPD = 0; |
| pinofal | 14:d58d474fae35 | 856 | LedWPS = 0; |
| pinofal | 14:d58d474fae35 | 857 | |
| pinofal | 14:d58d474fae35 | 858 | } |
| pinofal | 14:d58d474fae35 | 859 | } |
| pinofal | 14:d58d474fae35 | 860 | |
| pinofal | 14:d58d474fae35 | 861 | // invia il dato al PC |
| pinofal | 14:d58d474fae35 | 862 | //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight); |
| pinofal | 14:d58d474fae35 | 863 | //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++ |
| pinofal | 14:d58d474fae35 | 864 | |
| pinofal | 14:d58d474fae35 | 865 | //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++ |
| pinofal | 14:d58d474fae35 | 866 | //inizializza misura di distanza |
| pinofal | 14:d58d474fae35 | 867 | fDistance=0.0; |
| pinofal | 14:d58d474fae35 | 868 | // Fissa come Output il pin InOutProxSensor |
| pinofal | 14:d58d474fae35 | 869 | InOutProxSensor.output(); |
| pinofal | 14:d58d474fae35 | 870 | // Poni 'L' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 871 | InOutProxSensor.write(0); |
| pinofal | 14:d58d474fae35 | 872 | wait_us(5); |
| pinofal | 14:d58d474fae35 | 873 | // Poni 'H' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 874 | InOutProxSensor.write(1); |
| pinofal | 14:d58d474fae35 | 875 | wait_us(10); |
| pinofal | 14:d58d474fae35 | 876 | // Poni 'L' sul Pin e mantienilo per qualche microsecondo |
| pinofal | 14:d58d474fae35 | 877 | InOutProxSensor.write(0); |
| pinofal | 14:d58d474fae35 | 878 | // Attendi assestamento e Fissa come Input il pin InOutProxSensor |
| pinofal | 14:d58d474fae35 | 879 | wait_us(5); |
| pinofal | 14:d58d474fae35 | 880 | InOutProxSensor.input(); |
| pinofal | 14:d58d474fae35 | 881 | InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0' |
| pinofal | 14:d58d474fae35 | 882 | |
| pinofal | 14:d58d474fae35 | 883 | // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore |
| pinofal | 14:d58d474fae35 | 884 | TimerProxSensor.start(); |
| pinofal | 14:d58d474fae35 | 885 | nTimerStart = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 886 | nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart); |
| pinofal | 14:d58d474fae35 | 887 | while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR)) |
| pinofal | 14:d58d474fae35 | 888 | { |
| pinofal | 14:d58d474fae35 | 889 | nTimerCurrent = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 890 | nTimerTillNow=nTimerCurrent-nTimerStart; |
| pinofal | 14:d58d474fae35 | 891 | led2=1; // se rimane nel while il LED rimane acceso |
| pinofal | 14:d58d474fae35 | 892 | pc.printf("sono qui 2 \r\n"); |
| pinofal | 14:d58d474fae35 | 893 | } |
| pinofal | 14:d58d474fae35 | 894 | TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità |
| pinofal | 14:d58d474fae35 | 895 | pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow); |
| pinofal | 14:d58d474fae35 | 896 | // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo |
| pinofal | 14:d58d474fae35 | 897 | if(nTimerTillNow < TIMEOUTPROXSENSOR) |
| pinofal | 14:d58d474fae35 | 898 | { |
| pinofal | 14:d58d474fae35 | 899 | // riattiva il timer per misurare la distanza dell'ostacolo |
| pinofal | 14:d58d474fae35 | 900 | TimerProxSensor.start(); |
| pinofal | 14:d58d474fae35 | 901 | nTimerStart = TimerProxSensor.read_us(); |
| pinofal | 14:d58d474fae35 | 902 | while(InOutProxSensor == 1) |
| pinofal | 14:d58d474fae35 | 903 | { |
| pinofal | 14:d58d474fae35 | 904 | led2=1; // se rimane nel while il LED rimane acceso |
| pinofal | 14:d58d474fae35 | 905 | } |
| pinofal | 14:d58d474fae35 | 906 | TimerProxSensor.stop(); |
| pinofal | 14:d58d474fae35 | 907 | nTimerStop = TimerProxSensor.read_us(); |
| pinofal | 13:459e008582b3 | 908 | |
| pinofal | 14:d58d474fae35 | 909 | pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow); |
| pinofal | 14:d58d474fae35 | 910 | |
| pinofal | 14:d58d474fae35 | 911 | // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us] |
| pinofal | 14:d58d474fae35 | 912 | // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore |
| pinofal | 14:d58d474fae35 | 913 | // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us] |
| pinofal | 14:d58d474fae35 | 914 | fDistance = (nTimerStop-nTimerStart)/58.2; |
| pinofal | 14:d58d474fae35 | 915 | // invia il dato al PC |
| pinofal | 14:d58d474fae35 | 916 | pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance); |
| pinofal | 14:d58d474fae35 | 917 | } |
| pinofal | 14:d58d474fae35 | 918 | else |
| pinofal | 14:d58d474fae35 | 919 | { |
| pinofal | 14:d58d474fae35 | 920 | // quando esce dai while bloccanti, il LED si spegne |
| pinofal | 14:d58d474fae35 | 921 | led2=0; |
| pinofal | 14:d58d474fae35 | 922 | pc.printf("\r\nTimeOut\r\n"); |
| pinofal | 14:d58d474fae35 | 923 | } |
| pinofal | 14:d58d474fae35 | 924 | //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ |
| pinofal | 12:00ce5d30b82c | 925 | |
| pinofal | 12:00ce5d30b82c | 926 | |
| pinofal | 14:d58d474fae35 | 927 | //++++++++++++++ INIZIO Suona Clacson +++++++++ |
| pinofal | 14:d58d474fae35 | 928 | //escludi le misure oltre il max |
| pinofal | 14:d58d474fae35 | 929 | if((fDistance <= 50.0) && (fDistance >= 3)) |
| pinofal | 14:d58d474fae35 | 930 | { |
| pinofal | 12:00ce5d30b82c | 931 | // visualizza il valore misurato |
| pinofal | 12:00ce5d30b82c | 932 | printf("The Distance was %f [cm]\n\r", fDistance); |
| pinofal | 12:00ce5d30b82c | 933 | |
| pinofal | 12:00ce5d30b82c | 934 | // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima |
| pinofal | 12:00ce5d30b82c | 935 | if(fDistance < 22) |
| pinofal | 12:00ce5d30b82c | 936 | { |
| pinofal | 14:d58d474fae35 | 937 | // blocca altri suoni quando genera suono del clacson |
| pinofal | 14:d58d474fae35 | 938 | bEngineSoundStop=1; |
| pinofal | 12:00ce5d30b82c | 939 | // INIZIO generazione tono |
| pinofal | 14:d58d474fae35 | 940 | nClacsonSampleIndex=0; |
| pinofal | 14:d58d474fae35 | 941 | // Genera il suono del clacson |
| pinofal | 14:d58d474fae35 | 942 | for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++) |
| pinofal | 12:00ce5d30b82c | 943 | { |
| pinofal | 14:d58d474fae35 | 944 | OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767 |
| pinofal | 12:00ce5d30b82c | 945 | //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici |
| pinofal | 14:d58d474fae35 | 946 | wait(fDeltaTClacsonSound); |
| pinofal | 12:00ce5d30b82c | 947 | // genera ciclicamente |
| pinofal | 14:d58d474fae35 | 948 | nClacsonSampleIndex++; |
| pinofal | 14:d58d474fae35 | 949 | if(nClacsonSampleIndex >= CLACSONSAMPLENUM) |
| pinofal | 12:00ce5d30b82c | 950 | { |
| pinofal | 14:d58d474fae35 | 951 | nClacsonSampleIndex=0; |
| pinofal | 12:00ce5d30b82c | 952 | } |
| pinofal | 12:00ce5d30b82c | 953 | // a metà genera un wait per doppio clacson |
| pinofal | 14:d58d474fae35 | 954 | if(nClacsonSampleCount == 2000) |
| pinofal | 12:00ce5d30b82c | 955 | { |
| pinofal | 12:00ce5d30b82c | 956 | wait_ms(100); |
| pinofal | 12:00ce5d30b82c | 957 | } |
| pinofal | 14:d58d474fae35 | 958 | |
| pinofal | 12:00ce5d30b82c | 959 | } |
| pinofal | 12:00ce5d30b82c | 960 | //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente |
| pinofal | 12:00ce5d30b82c | 961 | OutWave.write_u16(0); |
| pinofal | 14:d58d474fae35 | 962 | |
| pinofal | 14:d58d474fae35 | 963 | // sblocca altri suoni dopo aver generato suono del clacson |
| pinofal | 14:d58d474fae35 | 964 | bEngineSoundStop=0; |
| pinofal | 14:d58d474fae35 | 965 | |
| pinofal | 12:00ce5d30b82c | 966 | } // if(fDistance < soglia) suona clacson |
| pinofal | 12:00ce5d30b82c | 967 | |
| pinofal | 14:d58d474fae35 | 968 | } // if( (fDistance < Max) && (fDistance > Min)) |
| pinofal | 12:00ce5d30b82c | 969 | //++++++++++++++ FINE Suona Clacson +++++++++ |
| pinofal | 14:d58d474fae35 | 970 | wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente |
| pinofal | 12:00ce5d30b82c | 971 | |
| pinofal | 12:00ce5d30b82c | 972 | } |
| pinofal | 12:00ce5d30b82c | 973 | //++++ FINE Ciclo Principale ++++ |
| pinofal | 12:00ce5d30b82c | 974 | } |
| pinofal | 12:00ce5d30b82c | 975 |