Giuseppe Falagario / Mbed 2 deprecated Amaldi_RobotFinale_Rev4

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed

RobotFinale3-2.cpp@15:ef808556423b, 2019-02-20 (annotated)

Committer:
pinofal
Date:
Wed Feb 20 21:01:29 2019 +0000
Revision:
15:ef808556423b
Prova per il clacson in movimento

Who changed what in which revision?

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