Amaldi / Mbed 2 deprecated Amaldi_RobotFinale_Rev2

Amaldi Robot Finale rev 2.0

Dependencies:   mbed X-NUCLEO-IHM05A1

RobotFinale.cpp@15:d7126015fe69, 2018-11-24 (annotated)

Committer:
francesco01
Date:
Sat Nov 24 15:46:05 2018 +0000
Revision:
15:d7126015fe69
Parent:
14:8e890b00c826 
modifiche motorino

Who changed what in which revision?

UserRevisionLine numberNew contents of line
francesco01 14:8e890b00c826 1 /*modifca pin led gialli
francesco01 14:8e890b00c826 2 modifica soglia luminsità*/
francesco01 14:8e890b00c826 3
francesco01 14:8e890b00c826 4
pinofal 12:00ce5d30b82c 5 /* mbed specific header files. */
pinofal 12:00ce5d30b82c 6 #include "mbed.h"
pinofal 12:00ce5d30b82c 7
pinofal 12:00ce5d30b82c 8 /* Component specific header files. */
pinofal 12:00ce5d30b82c 9 #include "L6208.h"
pinofal 12:00ce5d30b82c 10
pinofal 12:00ce5d30b82c 11 // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
pinofal 12:00ce5d30b82c 12 #define SAMPLESINENUM 45 // consigliabile avere multipli di 45
pinofal 12:00ce5d30b82c 13
pinofal 12:00ce5d30b82c 14 // numero di campioni acquisiti su cui effettuare la media di luminosità
pinofal 12:00ce5d30b82c 15 #define NUMLIGHTSAMPLE 100
pinofal 12:00ce5d30b82c 16
pinofal 12:00ce5d30b82c 17 // parametri dell'onda coseno da generare
pinofal 12:00ce5d30b82c 18 #define PI (3.141592653589793238462)
pinofal 12:00ce5d30b82c 19 #define AMPLITUDE 32767 //(1.0) // x * 3.3V
pinofal 12:00ce5d30b82c 20 #define PHASE (PI/2) // 2*pi è un periodo
pinofal 12:00ce5d30b82c 21 #define OFFSET 32767 //(0x7FFF)
pinofal 12:00ce5d30b82c 22
pinofal 12:00ce5d30b82c 23 // Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
francesco01 14:8e890b00c826 24 #define SOGLIALUCI (1.85)
pinofal 12:00ce5d30b82c 25
pinofal 12:00ce5d30b82c 26 // Definizione Parametri per il motore
pinofal 12:00ce5d30b82c 27 #ifdef TARGET_NUCLEO_F334R8
pinofal 12:00ce5d30b82c 28 #define VREFA_PWM_PIN D11
pinofal 12:00ce5d30b82c 29 #define VREFB_PWM_PIN D9
pinofal 12:00ce5d30b82c 30 #elif TARGET_NUCLEO_F302R8
pinofal 12:00ce5d30b82c 31 #define VREFA_PWM_PIN D11
pinofal 12:00ce5d30b82c 32 #define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
pinofal 12:00ce5d30b82c 33 #else
pinofal 12:00ce5d30b82c 34 #define VREFA_PWM_PIN D3
pinofal 12:00ce5d30b82c 35 #define VREFB_PWM_PIN D9
pinofal 12:00ce5d30b82c 36 #endif
pinofal 12:00ce5d30b82c 37
pinofal 12:00ce5d30b82c 38 // definizioni funzioni di default su scheda
pinofal 12:00ce5d30b82c 39 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
pinofal 12:00ce5d30b82c 40 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
pinofal 12:00ce5d30b82c 41 DigitalOut led2(LED2);// LED verde sulla scheda
pinofal 12:00ce5d30b82c 42
pinofal 12:00ce5d30b82c 43 // pin A2 di output per la forma d'onda analogica dedicata al suono
pinofal 12:00ce5d30b82c 44 AnalogOut OutWave(PA_4);
pinofal 12:00ce5d30b82c 45
pinofal 12:00ce5d30b82c 46 // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
pinofal 12:00ce5d30b82c 47 AnalogIn InWaveLight(PA_1);
pinofal 12:00ce5d30b82c 48
pinofal 12:00ce5d30b82c 49 // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
pinofal 12:00ce5d30b82c 50 DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0);
pinofal 12:00ce5d30b82c 51 // Timer per il calcolo dei tempi del sensore di prossimità
pinofal 12:00ce5d30b82c 52 Timer myTimer;
pinofal 12:00ce5d30b82c 53
pinofal 12:00ce5d30b82c 54 // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
pinofal 12:00ce5d30b82c 55 int nTimerStart, nTimer, nTimerStop;
pinofal 12:00ce5d30b82c 56
pinofal 12:00ce5d30b82c 57 // distanza in cm dell'ostacolo
pinofal 12:00ce5d30b82c 58 double fDistance;
pinofal 12:00ce5d30b82c 59
pinofal 12:00ce5d30b82c 60 // Buffer contenente la sinusoide da porre in output.
pinofal 12:00ce5d30b82c 61 unsigned short usaSine[SAMPLESINENUM];
pinofal 12:00ce5d30b82c 62
pinofal 12:00ce5d30b82c 63 // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
pinofal 12:00ce5d30b82c 64 void CalculateSinewave(void);
pinofal 12:00ce5d30b82c 65
pinofal 12:00ce5d30b82c 66
pinofal 12:00ce5d30b82c 67 // Periodo di generazione campioni in output DeltaT = T/NumSample
pinofal 12:00ce5d30b82c 68 double fDeltaT;
pinofal 12:00ce5d30b82c 69 // amplificazione per il dato da spedire sull'ADC
pinofal 12:00ce5d30b82c 70 double fAmp;
pinofal 12:00ce5d30b82c 71
pinofal 12:00ce5d30b82c 72 // frequenza segnale audio da generare
pinofal 12:00ce5d30b82c 73 double fFreq;
pinofal 12:00ce5d30b82c 74
pinofal 12:00ce5d30b82c 75 // periodo della sinusoide audio da generare
pinofal 12:00ce5d30b82c 76 double fPeriod;
pinofal 12:00ce5d30b82c 77
pinofal 12:00ce5d30b82c 78 // indice per i cicli
pinofal 12:00ce5d30b82c 79 int nIndex;
pinofal 12:00ce5d30b82c 80
pinofal 12:00ce5d30b82c 81 // numero di campioni di onda sonora già generati
pinofal 12:00ce5d30b82c 82 int nSampleCount;
pinofal 12:00ce5d30b82c 83
pinofal 12:00ce5d30b82c 84
pinofal 12:00ce5d30b82c 85 // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
pinofal 12:00ce5d30b82c 86 double fAvgLight;
pinofal 12:00ce5d30b82c 87
pinofal 12:00ce5d30b82c 88 // valore numerico, di tensione e di luce letto dall'ADC
pinofal 12:00ce5d30b82c 89 volatile unsigned short usReadADC;
pinofal 12:00ce5d30b82c 90 volatile float fReadVoltage;
pinofal 12:00ce5d30b82c 91
pinofal 12:00ce5d30b82c 92 // valore di luminosità letto dall'ADC
pinofal 12:00ce5d30b82c 93 volatile float fLight;
pinofal 12:00ce5d30b82c 94
pinofal 12:00ce5d30b82c 95 // Output Digitali usati per i LED
pinofal 12:00ce5d30b82c 96 DigitalOut LedWAD (PC_2);
pinofal 12:00ce5d30b82c 97 DigitalOut LedWAS (PC_3);
pinofal 13:459e008582b3 98 DigitalOut LedWPD (PH_0);
pinofal 12:00ce5d30b82c 99 DigitalOut LedWPS (PA_0) ;
francesco01 14:8e890b00c826 100 DigitalOut LedYAD (PC_9);
francesco01 14:8e890b00c826 101 DigitalOut LedYAS (PC_8);
pinofal 13:459e008582b3 102 DigitalOut LedRPD (PA_13);
pinofal 13:459e008582b3 103 DigitalOut LedRPS (PA_14) ;
francesco01 15:d7126015fe69 104 DigitalOut OutA (PB_0);
francesco01 15:d7126015fe69 105 DigitalOut OutB (PC_1);
pinofal 13:459e008582b3 106
pinofal 12:00ce5d30b82c 107 // Input/Output Digitali usati per interfaccia RPI
pinofal 12:00ce5d30b82c 108 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
pinofal 12:00ce5d30b82c 109 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
pinofal 12:00ce5d30b82c 110 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
pinofal 12:00ce5d30b82c 111 DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
pinofal 13:459e008582b3 112 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione = RPI GPIO12
pinofal 12:00ce5d30b82c 113
pinofal 12:00ce5d30b82c 114 /****************************************************************************/
pinofal 12:00ce5d30b82c 115 /* Initialization parameters of the motor connected to the expansion board. */
pinofal 12:00ce5d30b82c 116 /****************************************************************************/
pinofal 12:00ce5d30b82c 117 l6208_init_t init =
pinofal 12:00ce5d30b82c 118 {
pinofal 12:00ce5d30b82c 119
pinofal 12:00ce5d30b82c 120 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
pinofal 12:00ce5d30b82c 121 20, //Acceleration current torque in % (from 0 to 100)
pinofal 12:00ce5d30b82c 122 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
pinofal 12:00ce5d30b82c 123 30, //Deceleration current torque in % (from 0 to 100)
pinofal 12:00ce5d30b82c 124 1500, //Running speed in step/s or (1/16)th step/s for microstep modes
pinofal 12:00ce5d30b82c 125 50, //Running current torque in % (from 0 to 100)
pinofal 12:00ce5d30b82c 126 20, //Holding current torque in % (from 0 to 100)
pinofal 12:00ce5d30b82c 127 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
pinofal 12:00ce5d30b82c 128 FAST_DECAY, //Decay mode via enum motorDecayMode_t
pinofal 12:00ce5d30b82c 129 0, //Dwelling time in ms
pinofal 12:00ce5d30b82c 130 FALSE, //Automatic HIZ STOP
pinofal 12:00ce5d30b82c 131 100000 //VREFA and VREFB PWM frequency (Hz)
pinofal 12:00ce5d30b82c 132 };
pinofal 12:00ce5d30b82c 133
pinofal 12:00ce5d30b82c 134 /* Motor Control Component. */
pinofal 12:00ce5d30b82c 135 L6208 *motor;
pinofal 12:00ce5d30b82c 136
pinofal 12:00ce5d30b82c 137
pinofal 12:00ce5d30b82c 138 //****************************
pinofal 12:00ce5d30b82c 139 // Create the sinewave buffer
pinofal 12:00ce5d30b82c 140 //****************************
pinofal 12:00ce5d30b82c 141 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
pinofal 12:00ce5d30b82c 142 {
pinofal 12:00ce5d30b82c 143 // variabile contenente l'angolo in radianti
pinofal 12:00ce5d30b82c 144 double fRads;
pinofal 12:00ce5d30b82c 145 // indici per i cicli
pinofal 12:00ce5d30b82c 146 int nIndex;
pinofal 12:00ce5d30b82c 147 // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
pinofal 12:00ce5d30b82c 148 double fDeltaF;
pinofal 12:00ce5d30b82c 149 // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
pinofal 12:00ce5d30b82c 150 double fAngle;
pinofal 12:00ce5d30b82c 151
pinofal 12:00ce5d30b82c 152 fDeltaF = 360.0/SAMPLESINENUM;
pinofal 12:00ce5d30b82c 153 for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++)
pinofal 12:00ce5d30b82c 154 {
pinofal 12:00ce5d30b82c 155 fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
pinofal 12:00ce5d30b82c 156 fRads = (PI * fAngle)/180.0; // Convert degree in radian
pinofal 12:00ce5d30b82c 157 //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
pinofal 12:00ce5d30b82c 158 usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
pinofal 12:00ce5d30b82c 159 }
pinofal 12:00ce5d30b82c 160 }
pinofal 12:00ce5d30b82c 161
pinofal 12:00ce5d30b82c 162 /********************************************************/
pinofal 12:00ce5d30b82c 163 /* Funzione avviata all'inizio come saluto e Benvenuto */
pinofal 12:00ce5d30b82c 164 /********************************************************/
pinofal 12:00ce5d30b82c 165 void WelcomeMessage()
pinofal 12:00ce5d30b82c 166 {
pinofal 12:00ce5d30b82c 167 }
pinofal 12:00ce5d30b82c 168
pinofal 12:00ce5d30b82c 169 /********************************************************************/
pinofal 12:00ce5d30b82c 170 /* brief This is an example of user handler for the flag interrupt.*/
pinofal 12:00ce5d30b82c 171 /* param None */
pinofal 12:00ce5d30b82c 172 /* retval None */
pinofal 12:00ce5d30b82c 173 /* note If needed, implement it, and then attach and enable it: */
pinofal 12:00ce5d30b82c 174 /* + motor->attach_flag_irq(&my_flag_irq_handler); */
pinofal 12:00ce5d30b82c 175 /* + motor->enable_flag_irq(); */
pinofal 12:00ce5d30b82c 176 /* To disable it: */
pinofal 12:00ce5d30b82c 177 /* + motor->DisbleFlagIRQ(); */
pinofal 12:00ce5d30b82c 178 /*********************************************************************/
pinofal 12:00ce5d30b82c 179 void my_flag_irq_handler(void)
pinofal 12:00ce5d30b82c 180 {
pinofal 12:00ce5d30b82c 181 pc.printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
pinofal 12:00ce5d30b82c 182 motor->disable();
pinofal 12:00ce5d30b82c 183 pc.printf(" Motor disabled.\r\n\n");
pinofal 12:00ce5d30b82c 184 }
pinofal 12:00ce5d30b82c 185
pinofal 12:00ce5d30b82c 186 /***************************************************************
pinofal 12:00ce5d30b82c 187 * brief This is an example of error handler.
pinofal 12:00ce5d30b82c 188 * param[in] error Number of the error
pinofal 12:00ce5d30b82c 189 * retval None
pinofal 12:00ce5d30b82c 190 * note If needed, implement it, and then attach it:
pinofal 12:00ce5d30b82c 191 * + motor->attach_error_handler(&my_error_handler);
pinofal 12:00ce5d30b82c 192 **************************************************************/
pinofal 12:00ce5d30b82c 193 void my_error_handler(uint16_t error)
pinofal 12:00ce5d30b82c 194 {
pinofal 12:00ce5d30b82c 195 /* Printing to the console. */
pinofal 12:00ce5d30b82c 196 pc.printf("Error %d detected\r\n\n", error);
pinofal 12:00ce5d30b82c 197
pinofal 12:00ce5d30b82c 198 /* Infinite loop */
pinofal 12:00ce5d30b82c 199 while (true)
pinofal 12:00ce5d30b82c 200 {}
pinofal 12:00ce5d30b82c 201 }
pinofal 12:00ce5d30b82c 202
pinofal 12:00ce5d30b82c 203 /********/
pinofal 12:00ce5d30b82c 204 /* Main */
pinofal 12:00ce5d30b82c 205 /********/
pinofal 12:00ce5d30b82c 206 int main()
pinofal 12:00ce5d30b82c 207 {
pinofal 12:00ce5d30b82c 208 // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
pinofal 12:00ce5d30b82c 209 pc.baud(921600); //921600 bps
pinofal 12:00ce5d30b82c 210 //pc.baud(9600); //256000 bps
pinofal 12:00ce5d30b82c 211 pc.printf("*** Test Motor ***\n\r");
pinofal 12:00ce5d30b82c 212
pinofal 12:00ce5d30b82c 213 /* Initializing Motor Control Component. */
pinofal 12:00ce5d30b82c 214 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
pinofal 12:00ce5d30b82c 215 if (motor->init(&init) != COMPONENT_OK)
pinofal 12:00ce5d30b82c 216 {
pinofal 12:00ce5d30b82c 217 exit(EXIT_FAILURE);
pinofal 12:00ce5d30b82c 218 }
pinofal 12:00ce5d30b82c 219
pinofal 12:00ce5d30b82c 220 /* Attaching and enabling an interrupt handler. */
pinofal 12:00ce5d30b82c 221 motor->attach_flag_irq(&my_flag_irq_handler);
pinofal 12:00ce5d30b82c 222 motor->enable_flag_irq();
pinofal 12:00ce5d30b82c 223
pinofal 12:00ce5d30b82c 224 /* Attaching an error handler */
pinofal 12:00ce5d30b82c 225 motor->attach_error_handler(&my_error_handler);
pinofal 12:00ce5d30b82c 226
pinofal 12:00ce5d30b82c 227 /* Printing to the console. */
pinofal 12:00ce5d30b82c 228 pc.printf("Motor Control Application Example for 1 Motor\r\n");
pinofal 12:00ce5d30b82c 229
pinofal 12:00ce5d30b82c 230 //----- run the motor BACKWARD
pinofal 12:00ce5d30b82c 231 pc.printf("--> Running the motor backward.\r\n");
pinofal 12:00ce5d30b82c 232 motor->run(StepperMotor::BWD);
pinofal 12:00ce5d30b82c 233
pinofal 12:00ce5d30b82c 234
pinofal 12:00ce5d30b82c 235 //----- Soft stop required while running
pinofal 12:00ce5d30b82c 236 pc.printf("--> Soft stop requested.\r\n");
pinofal 12:00ce5d30b82c 237 motor->soft_stop();
pinofal 12:00ce5d30b82c 238
pinofal 12:00ce5d30b82c 239
pinofal 12:00ce5d30b82c 240 //----- Change step mode to full step mode
pinofal 12:00ce5d30b82c 241 motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
pinofal 12:00ce5d30b82c 242 pc.printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->get_step_mode());
pinofal 12:00ce5d30b82c 243
pinofal 12:00ce5d30b82c 244 /* Get current position of device and print to the console */
pinofal 12:00ce5d30b82c 245 pc.printf(" Position: %d.\r\n", motor->get_position());
pinofal 12:00ce5d30b82c 246
pinofal 12:00ce5d30b82c 247 /* Set speed, acceleration and deceleration to scale with normal mode */
pinofal 12:00ce5d30b82c 248 motor->set_max_speed(init.maxSpeedSps>>4);
pinofal 12:00ce5d30b82c 249 motor->set_acceleration(motor->get_acceleration()>>4);
pinofal 12:00ce5d30b82c 250 motor->set_deceleration(motor->get_deceleration()>>4);
pinofal 12:00ce5d30b82c 251 /* Print parameters to the console */
pinofal 12:00ce5d30b82c 252 pc.printf(" Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
pinofal 12:00ce5d30b82c 253 pc.printf(" Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
pinofal 12:00ce5d30b82c 254 pc.printf(" Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
pinofal 12:00ce5d30b82c 255 pc.printf(" Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
pinofal 12:00ce5d30b82c 256
pinofal 12:00ce5d30b82c 257 //----- move of 200 steps in the FW direction
pinofal 12:00ce5d30b82c 258 pc.printf("--> Moving forward 200 steps.\r\n");
pinofal 12:00ce5d30b82c 259 motor->move(StepperMotor::FWD, 200);
pinofal 12:00ce5d30b82c 260
pinofal 12:00ce5d30b82c 261 //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
pinofal 12:00ce5d30b82c 262 fFreq = 440.0; // frequenza in Hz del tono da generare
pinofal 12:00ce5d30b82c 263 fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
pinofal 12:00ce5d30b82c 264 fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
pinofal 12:00ce5d30b82c 265 CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
pinofal 12:00ce5d30b82c 266 //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
pinofal 12:00ce5d30b82c 267
francesco01 15:d7126015fe69 268
pinofal 12:00ce5d30b82c 269 while(true)
pinofal 12:00ce5d30b82c 270 {
francesco01 15:d7126015fe69 271 if(myButton==0)
francesco01 15:d7126015fe69 272 {
francesco01 15:d7126015fe69 273 //Ferma motore
francesco01 15:d7126015fe69 274 OutA=0;
francesco01 15:d7126015fe69 275 OutB=0;
francesco01 15:d7126015fe69 276 pc.printf("OutA OutB = 00\r\n");
francesco01 15:d7126015fe69 277 wait_ms(2000);
francesco01 15:d7126015fe69 278
francesco01 15:d7126015fe69 279
francesco01 15:d7126015fe69 280 // Ruota Left
francesco01 15:d7126015fe69 281 OutA=0;
francesco01 15:d7126015fe69 282 OutB=1;
francesco01 15:d7126015fe69 283 pc.printf("OutA OutB = 01\r\n");
francesco01 15:d7126015fe69 284 wait_ms(2000);
francesco01 15:d7126015fe69 285 //Ferma motore
francesco01 15:d7126015fe69 286 OutA=0;
francesco01 15:d7126015fe69 287 OutB=0;
francesco01 15:d7126015fe69 288 pc.printf("OutA OutB = 00\r\n");
francesco01 15:d7126015fe69 289 wait_ms(2000);
francesco01 15:d7126015fe69 290 }
francesco01 15:d7126015fe69 291 else
francesco01 15:d7126015fe69 292 {
francesco01 15:d7126015fe69 293 //Ferma motore
francesco01 15:d7126015fe69 294 OutA=1;
francesco01 15:d7126015fe69 295 OutB=0;
francesco01 15:d7126015fe69 296 pc.printf("OutA OutB = 10\r\n");
francesco01 15:d7126015fe69 297 wait_ms(2000);
francesco01 15:d7126015fe69 298
francesco01 15:d7126015fe69 299 // Ruota Right
francesco01 15:d7126015fe69 300 OutA=1;
francesco01 15:d7126015fe69 301 OutB=1;
francesco01 15:d7126015fe69 302 pc.printf("OutA OutB = 11\r\n");
francesco01 15:d7126015fe69 303 wait_ms(2000);
francesco01 15:d7126015fe69 304
francesco01 15:d7126015fe69 305 OutA=1;
francesco01 15:d7126015fe69 306 OutB=0;
francesco01 15:d7126015fe69 307 pc.printf("OutA OutB = 10\r\n");
francesco01 15:d7126015fe69 308 wait_ms(2000);
pinofal 12:00ce5d30b82c 309 }
francesco01 15:d7126015fe69 310
pinofal 12:00ce5d30b82c 311
pinofal 12:00ce5d30b82c 312 //++++ INIZIO Ciclo Principale ++++
pinofal 12:00ce5d30b82c 313 while (true)
pinofal 12:00ce5d30b82c 314 {
pinofal 12:00ce5d30b82c 315 //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
pinofal 12:00ce5d30b82c 316 if(InMotorSwitchRPI==1)
pinofal 12:00ce5d30b82c 317 {
francesco01 15:d7126015fe69 318 //Ferma motore
francesco01 15:d7126015fe69 319 OutA=0;
francesco01 15:d7126015fe69 320 OutB=0;
francesco01 15:d7126015fe69 321 pc.printf("OutA OutB = 00\r\n");
francesco01 15:d7126015fe69 322 wait_ms(2000);
francesco01 15:d7126015fe69 323
francesco01 15:d7126015fe69 324
francesco01 15:d7126015fe69 325 // Ruota Left
francesco01 15:d7126015fe69 326 OutA=0;
francesco01 15:d7126015fe69 327 OutB=1;
francesco01 15:d7126015fe69 328 pc.printf("OutA OutB = 01\r\n");
francesco01 15:d7126015fe69 329 wait_ms(2000);
francesco01 15:d7126015fe69 330 //Ferma motore
francesco01 15:d7126015fe69 331 OutA=0;
francesco01 15:d7126015fe69 332 OutB=0;
francesco01 15:d7126015fe69 333 pc.printf("OutA OutB = 00\r\n");
francesco01 15:d7126015fe69 334 wait_ms(2000);
pinofal 12:00ce5d30b82c 335 }
pinofal 12:00ce5d30b82c 336 else
pinofal 12:00ce5d30b82c 337 {
francesco01 15:d7126015fe69 338 //Ferma motore
francesco01 15:d7126015fe69 339 OutA=1;
francesco01 15:d7126015fe69 340 OutB=0;
francesco01 15:d7126015fe69 341 pc.printf("OutA OutB = 10\r\n");
francesco01 15:d7126015fe69 342 wait_ms(2000);
francesco01 15:d7126015fe69 343
francesco01 15:d7126015fe69 344 // Ruota Right
francesco01 15:d7126015fe69 345 OutA=1;
francesco01 15:d7126015fe69 346 OutB=1;
francesco01 15:d7126015fe69 347 pc.printf("OutA OutB = 11\r\n");
francesco01 15:d7126015fe69 348 wait_ms(2000);
francesco01 15:d7126015fe69 349
francesco01 15:d7126015fe69 350 OutA=1;
francesco01 15:d7126015fe69 351 OutB=0;
francesco01 15:d7126015fe69 352 pc.printf("OutA OutB = 10\r\n");
francesco01 15:d7126015fe69 353 wait_ms(2000);
pinofal 12:00ce5d30b82c 354 }
pinofal 12:00ce5d30b82c 355 //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
pinofal 12:00ce5d30b82c 356 //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
pinofal 12:00ce5d30b82c 357 if(InLightSwitchRPI ==1)
pinofal 12:00ce5d30b82c 358 {
pinofal 13:459e008582b3 359
pinofal 12:00ce5d30b82c 360 // accendi i LED di abbellimento
pinofal 13:459e008582b3 361 //led2=1;
pinofal 13:459e008582b3 362 LedYAD = 1;
pinofal 13:459e008582b3 363 LedYAS = 1;
pinofal 13:459e008582b3 364 LedRPD = 1;
pinofal 13:459e008582b3 365 LedRPS = 1;
pinofal 13:459e008582b3 366
francesco01 14:8e890b00c826 367
pinofal 12:00ce5d30b82c 368 }
pinofal 12:00ce5d30b82c 369 else
pinofal 12:00ce5d30b82c 370 {
pinofal 13:459e008582b3 371
pinofal 12:00ce5d30b82c 372 // spegni i LED di abbellimento
pinofal 13:459e008582b3 373 //led2=0;
pinofal 13:459e008582b3 374 LedYAD = 0;
pinofal 13:459e008582b3 375 LedYAS = 0;
pinofal 13:459e008582b3 376 LedRPD = 0;
pinofal 13:459e008582b3 377 LedRPS = 0;
francesco01 14:8e890b00c826 378
pinofal 13:459e008582b3 379
pinofal 12:00ce5d30b82c 380 }
pinofal 12:00ce5d30b82c 381 //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
pinofal 13:459e008582b3 382
pinofal 13:459e008582b3 383 //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
pinofal 12:00ce5d30b82c 384 // inizializza il valore medio della Luminosità
pinofal 12:00ce5d30b82c 385 fAvgLight=0.0;
pinofal 12:00ce5d30b82c 386 for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
pinofal 12:00ce5d30b82c 387 {
pinofal 12:00ce5d30b82c 388 // acquisisce dato da ADC
pinofal 12:00ce5d30b82c 389 usReadADC = InWaveLight.read_u16();
pinofal 12:00ce5d30b82c 390 fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
pinofal 12:00ce5d30b82c 391 //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
pinofal 12:00ce5d30b82c 392 fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
pinofal 12:00ce5d30b82c 393 fAvgLight+=fLight;
pinofal 12:00ce5d30b82c 394 }
pinofal 12:00ce5d30b82c 395 // calcola valore medio su NUMSAMPLE acquisizioni
pinofal 12:00ce5d30b82c 396 fAvgLight/= NUMLIGHTSAMPLE;
pinofal 12:00ce5d30b82c 397 // Accendi LED Bianchi se illuminazione è sottosoglia
pinofal 12:00ce5d30b82c 398 if(fAvgLight < SOGLIALUCI)
pinofal 12:00ce5d30b82c 399 {
pinofal 13:459e008582b3 400
pinofal 13:459e008582b3 401 // Accendi LED Bianchi
pinofal 13:459e008582b3 402 //led2 = 1;
pinofal 13:459e008582b3 403 LedWAD = 1;
pinofal 13:459e008582b3 404 LedWAS = 1;
pinofal 13:459e008582b3 405 LedWPD = 1;
pinofal 13:459e008582b3 406 LedWPS = 1;
pinofal 13:459e008582b3 407
pinofal 12:00ce5d30b82c 408 }
pinofal 12:00ce5d30b82c 409 else
pinofal 12:00ce5d30b82c 410 {
pinofal 13:459e008582b3 411
pinofal 13:459e008582b3 412 // Spegni LED Bianchi
pinofal 13:459e008582b3 413 //led2 = 0;
pinofal 13:459e008582b3 414 LedWAD = 0;
pinofal 13:459e008582b3 415 LedWAS = 0;
pinofal 13:459e008582b3 416 LedWPD = 0;
pinofal 13:459e008582b3 417 LedWPS = 0;
pinofal 13:459e008582b3 418
pinofal 12:00ce5d30b82c 419 }
pinofal 12:00ce5d30b82c 420
pinofal 12:00ce5d30b82c 421 // invia il dato al PC
pinofal 12:00ce5d30b82c 422 //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
pinofal 12:00ce5d30b82c 423 //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
pinofal 12:00ce5d30b82c 424
pinofal 12:00ce5d30b82c 425
pinofal 12:00ce5d30b82c 426 //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
pinofal 12:00ce5d30b82c 427 //inizializza misura di distanza
pinofal 12:00ce5d30b82c 428 fDistance=0.0;
pinofal 12:00ce5d30b82c 429 // Fissa come Output il pin myProx
pinofal 12:00ce5d30b82c 430 myProx.output();
pinofal 12:00ce5d30b82c 431 // Poni 'L' sul Pin e mantienilo per qualche microsecondo
pinofal 12:00ce5d30b82c 432 myProx.write(0);
pinofal 12:00ce5d30b82c 433 wait_us(5);
pinofal 12:00ce5d30b82c 434 // Poni 'H' sul Pin e mantienilo per qualche microsecondo
pinofal 12:00ce5d30b82c 435 myProx.write(1);
pinofal 12:00ce5d30b82c 436 wait_us(10);
pinofal 12:00ce5d30b82c 437 // Poni 'L' sul Pin e mantienilo per qualche microsecondo
pinofal 12:00ce5d30b82c 438 myProx.write(0);
pinofal 12:00ce5d30b82c 439 // Attendi assestamento e Fissa come Input il pin myProx
pinofal 12:00ce5d30b82c 440 wait_us(5);
pinofal 12:00ce5d30b82c 441 myProx.input();
pinofal 12:00ce5d30b82c 442
pinofal 12:00ce5d30b82c 443 // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
pinofal 12:00ce5d30b82c 444 nTimer =0;
pinofal 12:00ce5d30b82c 445 /*
pinofal 12:00ce5d30b82c 446 myTimer.start(); // avvia il timer per verificare la presenza del sensore
pinofal 12:00ce5d30b82c 447 while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
pinofal 12:00ce5d30b82c 448 {
pinofal 12:00ce5d30b82c 449 // misura il tempo passato
pinofal 12:00ce5d30b82c 450 nTimer=myTimer.read_us();
pinofal 12:00ce5d30b82c 451 }
pinofal 12:00ce5d30b82c 452 */
pinofal 12:00ce5d30b82c 453 while(myProx ==0)
pinofal 12:00ce5d30b82c 454 {}
pinofal 12:00ce5d30b82c 455 myTimer.stop(); // in ogni caso ferma il timer
pinofal 12:00ce5d30b82c 456 // vai avanti solo se il sensore ha risposto
pinofal 12:00ce5d30b82c 457 //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
pinofal 12:00ce5d30b82c 458 {
pinofal 12:00ce5d30b82c 459 myTimer.start();
pinofal 12:00ce5d30b82c 460 nTimerStart = myTimer.read_us();
pinofal 12:00ce5d30b82c 461 while(myProx == 1)
pinofal 12:00ce5d30b82c 462 {}
pinofal 12:00ce5d30b82c 463 myTimer.stop();
pinofal 12:00ce5d30b82c 464 nTimerStop = myTimer.read_us();
pinofal 12:00ce5d30b82c 465
pinofal 12:00ce5d30b82c 466
pinofal 12:00ce5d30b82c 467 // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
pinofal 12:00ce5d30b82c 468 // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
pinofal 12:00ce5d30b82c 469 // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
pinofal 12:00ce5d30b82c 470 fDistance = (nTimerStop-nTimerStart)/58.2;
pinofal 12:00ce5d30b82c 471 // invia il dato al PC
pinofal 12:00ce5d30b82c 472 //pc.printf("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
pinofal 12:00ce5d30b82c 473
pinofal 12:00ce5d30b82c 474 }
pinofal 12:00ce5d30b82c 475 //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
pinofal 12:00ce5d30b82c 476
pinofal 12:00ce5d30b82c 477 //++++++++++++++ INIZIO Suona Clacson +++++++++
pinofal 12:00ce5d30b82c 478 //escludi le misure oltre il max
pinofal 12:00ce5d30b82c 479 if((fDistance <= 50.0) && (fDistance >= 3))
pinofal 12:00ce5d30b82c 480 {
pinofal 12:00ce5d30b82c 481 // visualizza il valore misurato
pinofal 12:00ce5d30b82c 482 printf("The Distance was %f [cm]\n\r", fDistance);
pinofal 12:00ce5d30b82c 483
pinofal 12:00ce5d30b82c 484 // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
pinofal 12:00ce5d30b82c 485 if(fDistance < 22)
pinofal 12:00ce5d30b82c 486 {
pinofal 12:00ce5d30b82c 487 // INIZIO generazione tono
pinofal 12:00ce5d30b82c 488 nIndex=0;
pinofal 12:00ce5d30b82c 489 //Genera il suono del clacson
pinofal 12:00ce5d30b82c 490 for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
pinofal 12:00ce5d30b82c 491 {
pinofal 12:00ce5d30b82c 492 OutWave.write_u16(usaSine[nIndex]); //max 32767
pinofal 12:00ce5d30b82c 493 //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
pinofal 12:00ce5d30b82c 494 wait(fDeltaT);
pinofal 12:00ce5d30b82c 495 // genera ciclicamente
pinofal 12:00ce5d30b82c 496 nIndex++;
pinofal 12:00ce5d30b82c 497 if(nIndex >= SAMPLESINENUM)
pinofal 12:00ce5d30b82c 498 {
pinofal 12:00ce5d30b82c 499 nIndex=0;
pinofal 12:00ce5d30b82c 500 }
pinofal 12:00ce5d30b82c 501 // a metà genera un wait per doppio clacson
pinofal 12:00ce5d30b82c 502 if(nSampleCount == 2000)
pinofal 12:00ce5d30b82c 503 {
pinofal 12:00ce5d30b82c 504 wait_ms(100);
pinofal 12:00ce5d30b82c 505 }
pinofal 12:00ce5d30b82c 506
pinofal 12:00ce5d30b82c 507 }
pinofal 12:00ce5d30b82c 508 //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
pinofal 12:00ce5d30b82c 509 OutWave.write_u16(0);
pinofal 12:00ce5d30b82c 510
pinofal 12:00ce5d30b82c 511 } // if(fDistance < soglia) suona clacson
pinofal 12:00ce5d30b82c 512
pinofal 12:00ce5d30b82c 513 } // if( (fDistance < Max) && (fDistance > Min))
pinofal 12:00ce5d30b82c 514 //++++++++++++++ FINE Suona Clacson +++++++++
pinofal 12:00ce5d30b82c 515 wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
pinofal 12:00ce5d30b82c 516
pinofal 12:00ce5d30b82c 517 }
pinofal 12:00ce5d30b82c 518 //++++ FINE Ciclo Principale ++++
pinofal 12:00ce5d30b82c 519 }
francesco01 15:d7126015fe69 520 }