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