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