Amaldi / Mbed OS Amaldi_RobotFinale_Rev3

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.

RobotFinale3.cpp@0:74808e8c8d35, 2019-02-14 (annotated)

Committer:
pinofal
Date:
Thu Feb 14 09:31:28 2019 +0000
Revision:
0:74808e8c8d35
Robot ver. 3

Who changed what in which revision?

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