Giuseppe Falagario / Mbed 2 deprecated Amaldi_RobotFinale_Rev4

Robot Amaldi Rev 4.0 Messaggio don't touch me

Dependencies:   mbed

RobotFinale3-4.cpp@17:5f43dd94fc17, 2019-03-11 (annotated)

Committer:
pinofal
Date:
Mon Mar 11 01:09:34 2019 +0000
Revision:
17:5f43dd94fc17
Parent:
RobotFinale3-3.cpp@16:17df60afe302 
Amaldi rev 3.41 test per i rumori

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