Progetto STM32 / Mbed 2 deprecated ConcorsoFinal

concorso

Dependencies:   HCSR04 NetworkSocketAPI Servo X_NUCLEO_53L0A1 X_NUCLEO_IDW01M1v2 X_NUCLEO_IHM02A1 mbed-rtos mbed

Fork of HelloWorld_IHM02A1 by ST

main.cpp@27:4e679fecd547, 2017-04-29 (annotated)

Committer:
d3dfantasy99
Date:
Sat Apr 29 06:15:01 2017 +0000
Revision:
27:4e679fecd547
Parent:
26:caec5f51abe8 
concorso

Who changed what in which revision?

UserRevisionLine numberNew contents of line
d3dfantasy99 27:4e679fecd547 1 // Importo Librerie
d3dfantasy99 27:4e679fecd547 2 #include "mbed.h"
d3dfantasy99 27:4e679fecd547 3 /*#include "hcsr04.h"
d3dfantasy99 27:4e679fecd547 4 #include "SpwfInterface.h"
d3dfantasy99 27:4e679fecd547 5 #include "TCPSocket.h"
d3dfantasy99 27:4e679fecd547 6 */#include "DevSPI.h"
d3dfantasy99 27:4e679fecd547 7 /*#include "XNucleoIHM02A1.h"
d3dfantasy99 27:4e679fecd547 8 /*#include "x_nucleo_53l0a1.h"
d3dfantasy99 27:4e679fecd547 9 #include <stdio.h>
d3dfantasy99 27:4e679fecd547 10 #include "Servo.h"
d3dfantasy99 27:4e679fecd547 11 #include "rtos.h"*/
Davidroid 0:5148e9486cf2 12
Davidroid 0:5148e9486cf2 13 #define MPR_1 4
d3dfantasy99 27:4e679fecd547 14 #define STEPS_1 (400 * 128)
Davidroid 1:9f1974b0960d 15 #define DELAY_1 1000
Davidroid 0:5148e9486cf2 16 #define DELAY_2 2000
Davidroid 0:5148e9486cf2 17 #define DELAY_3 5000
d3dfantasy99 27:4e679fecd547 18 /*
d3dfantasy99 27:4e679fecd547 19 #define VL53L0_I2C_SDA D14 //sensore fotoni
d3dfantasy99 27:4e679fecd547 20 #define VL53L0_I2C_SCL D15 //sensore fotoni
d3dfantasy99 27:4e679fecd547 21 */
d3dfantasy99 27:4e679fecd547 22
d3dfantasy99 27:4e679fecd547 23
d3dfantasy99 27:4e679fecd547 24
d3dfantasy99 27:4e679fecd547 25
d3dfantasy99 27:4e679fecd547 26
d3dfantasy99 27:4e679fecd547 27
Davidroid 0:5148e9486cf2 28
Davidroid 0:5148e9486cf2 29
d3dfantasy99 27:4e679fecd547 30 DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
d3dfantasy99 27:4e679fecd547 31 static X_NUCLEO_53L0A1 *board=NULL; //sensore fotoni
d3dfantasy99 27:4e679fecd547 32 /*
d3dfantasy99 27:4e679fecd547 33 //Inizializzo le schede
d3dfantasy99 27:4e679fecd547 34 Serial pc(USBTX, USBRX); //seriale
d3dfantasy99 27:4e679fecd547 35 SpwfSAInterface wifi(D8, D2, false); //wifi
d3dfantasy99 27:4e679fecd547 36 Servo servomotore(PB_10); //servomotore
d3dfantasy99 27:4e679fecd547 37 PwmOut motori(PA_8);//D13
d3dfantasy99 27:4e679fecd547 38 DigitalOut sinistra(PA_0);
d3dfantasy99 27:4e679fecd547 39 DigitalOut destra(PA_1);
d3dfantasy99 27:4e679fecd547 40 TCPSocket socket(&wifi);
d3dfantasy99 27:4e679fecd547 41
Davidroid 0:5148e9486cf2 42
d3dfantasy99 27:4e679fecd547 43 //Variabili Globali
d3dfantasy99 27:4e679fecd547 44 char * ssid = "TekSmartLab"; //ssid wifi
d3dfantasy99 27:4e679fecd547 45 char * seckey = ""; //password wifi
d3dfantasy99 27:4e679fecd547 46 char * ip_socket = "192.168.1.104"; //ip socket master
d3dfantasy99 27:4e679fecd547 47 int porta_socket = 8000; //porta del socket
d3dfantasy99 27:4e679fecd547 48 int stato_socket = 0;
d3dfantasy99 27:4e679fecd547 49 int stato = 0; // 1 = start - 0 = stop (default stop)
d3dfantasy99 27:4e679fecd547 50 int gradi_servo = 45;
d3dfantasy99 27:4e679fecd547 51 char buffer[0];
d3dfantasy99 27:4e679fecd547 52 int controllo_buffer_in_arrivo = 0;
d3dfantasy99 27:4e679fecd547 53 int contatore_buff = 0;
d3dfantasy99 27:4e679fecd547 54 int invio_dati = 0;
d3dfantasy99 27:4e679fecd547 55 int buca_trovata = 0;
d3dfantasy99 27:4e679fecd547 56 int start_servo = 0;
d3dfantasy99 27:4e679fecd547 57 uint32_t distanza_centrale = 0;
d3dfantasy99 27:4e679fecd547 58 uint32_t distanza_destra = 0;
d3dfantasy99 27:4e679fecd547 59 uint32_t distanza_sinistra = 0;
d3dfantasy99 27:4e679fecd547 60 uint32_t test_distanza = 0;
d3dfantasy99 27:4e679fecd547 61 int misure[2];*/
Davidroid 26:caec5f51abe8 62 XNucleoIHM02A1 *x_nucleo_ihm02a1;
davide.aliprandi@st.com 24:d1f487cb02ba 63 L6470_init_t init[L6470DAISYCHAINSIZE] = {
Davidroid 9:f35fbeedb8f4 64 /* First Motor. */
Davidroid 9:f35fbeedb8f4 65 {
Davidroid 18:591a007effc2 66 9.0, /* Motor supply voltage in V. */
Davidroid 18:591a007effc2 67 400, /* Min number of steps per revolution for the motor. */
Davidroid 18:591a007effc2 68 1.7, /* Max motor phase voltage in A. */
Davidroid 18:591a007effc2 69 3.06, /* Max motor phase voltage in V. */
Davidroid 18:591a007effc2 70 300.0, /* Motor initial speed [step/s]. */
Davidroid 18:591a007effc2 71 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Davidroid 18:591a007effc2 72 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Davidroid 18:591a007effc2 73 992.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 74 0.0, /* Motor minimum speed [step/s]. */
Davidroid 18:591a007effc2 75 602.7, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 76 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 77 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 78 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 79 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 80 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 81 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 82 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 83 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 84 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 85 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 86 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 87 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Davidroid 18:591a007effc2 88 0xFF, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 89 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 90 },
Davidroid 9:f35fbeedb8f4 91
Davidroid 9:f35fbeedb8f4 92 /* Second Motor. */
Davidroid 9:f35fbeedb8f4 93 {
Davidroid 18:591a007effc2 94 9.0, /* Motor supply voltage in V. */
Davidroid 18:591a007effc2 95 400, /* Min number of steps per revolution for the motor. */
Davidroid 18:591a007effc2 96 1.7, /* Max motor phase voltage in A. */
Davidroid 18:591a007effc2 97 3.06, /* Max motor phase voltage in V. */
Davidroid 18:591a007effc2 98 300.0, /* Motor initial speed [step/s]. */
Davidroid 18:591a007effc2 99 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
Davidroid 18:591a007effc2 100 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
Davidroid 18:591a007effc2 101 992.0, /* Motor maximum speed [step/s]. */
Davidroid 18:591a007effc2 102 0.0, /* Motor minimum speed [step/s]. */
Davidroid 18:591a007effc2 103 602.7, /* Motor full-step speed threshold [step/s]. */
Davidroid 18:591a007effc2 104 3.06, /* Holding kval [V]. */
Davidroid 18:591a007effc2 105 3.06, /* Constant speed kval [V]. */
Davidroid 18:591a007effc2 106 3.06, /* Acceleration starting kval [V]. */
Davidroid 18:591a007effc2 107 3.06, /* Deceleration starting kval [V]. */
Davidroid 18:591a007effc2 108 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
Davidroid 18:591a007effc2 109 392.1569e-6, /* Start slope [s/step]. */
Davidroid 18:591a007effc2 110 643.1372e-6, /* Acceleration final slope [s/step]. */
Davidroid 18:591a007effc2 111 643.1372e-6, /* Deceleration final slope [s/step]. */
Davidroid 18:591a007effc2 112 0, /* Thermal compensation factor (range [0, 15]). */
Davidroid 18:591a007effc2 113 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
Davidroid 18:591a007effc2 114 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
Davidroid 18:591a007effc2 115 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
Davidroid 18:591a007effc2 116 0xFF, /* Alarm conditions enable. */
Davidroid 18:591a007effc2 117 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 118 }
Davidroid 9:f35fbeedb8f4 119 };
Davidroid 9:f35fbeedb8f4 120
d3dfantasy99 27:4e679fecd547 121
Davidroid 0:5148e9486cf2 122
d3dfantasy99 27:4e679fecd547 123 /*
d3dfantasy99 27:4e679fecd547 124
d3dfantasy99 27:4e679fecd547 125 void controllo_stato_connessione_thread(void const *args) {
d3dfantasy99 27:4e679fecd547 126 while(1){
d3dfantasy99 27:4e679fecd547 127 while(stato_socket == 0) {
d3dfantasy99 27:4e679fecd547 128
Davidroid 0:5148e9486cf2 129
d3dfantasy99 27:4e679fecd547 130 /* Initializing Motor Control Component. */
d3dfantasy99 27:4e679fecd547 131
d3dfantasy99 27:4e679fecd547 132
d3dfantasy99 27:4e679fecd547 133 /* Attaching and enabling interrupt handlers. */
d3dfantasy99 27:4e679fecd547 134
d3dfantasy99 27:4e679fecd547 135 /*
d3dfantasy99 27:4e679fecd547 136 board=X_NUCLEO_53L0A1::Instance(device_i2c, A2, D10, D9);
d3dfantasy99 27:4e679fecd547 137 board->InitBoard();
d3dfantasy99 27:4e679fecd547 138 DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
d3dfantasy99 27:4e679fecd547 139 board->sensor_centre->GetDistance(&test_distanza);
d3dfantasy99 27:4e679fecd547 140
d3dfantasy99 27:4e679fecd547 141 servomotore = gradi_servo/100.0;
d3dfantasy99 27:4e679fecd547 142 destra.write(0);
d3dfantasy99 27:4e679fecd547 143 sinistra.write(0);
d3dfantasy99 27:4e679fecd547 144 pc.printf("\r\n Rift Searcher 1.0 \r\n");
d3dfantasy99 27:4e679fecd547 145 /* while(!wifi.connect(ssid, seckey, NSAPI_SECURITY_NONE))/*NSAPI_SECURITY_WPA2*/ {
d3dfantasy99 27:4e679fecd547 146 /* pc.printf("\r\n Connessione alla rete wifi corso... Attendere...\r\n");
d3dfantasy99 27:4e679fecd547 147 wait_ms(2000);
d3dfantasy99 27:4e679fecd547 148 }
d3dfantasy99 27:4e679fecd547 149 //connesso alla rete wifi
d3dfantasy99 27:4e679fecd547 150 pc.printf("\r\n Connesso alla rete Wifi\r\n");
d3dfantasy99 27:4e679fecd547 151
d3dfantasy99 27:4e679fecd547 152 //verifico la connessione al socket
d3dfantasy99 27:4e679fecd547 153
d3dfantasy99 27:4e679fecd547 154 while(socket.connect(ip_socket, porta_socket) != 0){
d3dfantasy99 27:4e679fecd547 155 pc.printf("\r\n Connessione al socket corso... Attendere...\r\n");
d3dfantasy99 27:4e679fecd547 156 wait_ms(2000);
d3dfantasy99 27:4e679fecd547 157 }
d3dfantasy99 27:4e679fecd547 158 //connesso al socket
d3dfantasy99 27:4e679fecd547 159 pc.printf("\r\n Connessione al socket riuscita\r\n");
d3dfantasy99 27:4e679fecd547 160 stato_socket = 1;
d3dfantasy99 27:4e679fecd547 161 }
d3dfantasy99 27:4e679fecd547 162 }
d3dfantasy99 27:4e679fecd547 163 }
Davidroid 1:9f1974b0960d 164
d3dfantasy99 27:4e679fecd547 165 void controllo_dati_entrata_thread(void const *args) {
d3dfantasy99 27:4e679fecd547 166 pc.printf("\r\n Controllo dati\r\n");
d3dfantasy99 27:4e679fecd547 167 while(1){
d3dfantasy99 27:4e679fecd547 168 if(stato_socket == 1){
d3dfantasy99 27:4e679fecd547 169 char buffer[1];
d3dfantasy99 27:4e679fecd547 170 if(socket.recv(buffer, sizeof buffer) != 0){
d3dfantasy99 27:4e679fecd547 171 int count = 0;
d3dfantasy99 27:4e679fecd547 172 count = socket.recv(buffer, sizeof buffer);
d3dfantasy99 27:4e679fecd547 173 if(count > 0){
d3dfantasy99 27:4e679fecd547 174 buffer [count]='\0';
d3dfantasy99 27:4e679fecd547 175 if(buffer[0] == '0'){
d3dfantasy99 27:4e679fecd547 176 stato = 0;
d3dfantasy99 27:4e679fecd547 177 pc.printf("\r\n Stato 0\r\n");
d3dfantasy99 27:4e679fecd547 178 }
d3dfantasy99 27:4e679fecd547 179 else if (buffer[0] == '1'){
d3dfantasy99 27:4e679fecd547 180 stato = 1;
d3dfantasy99 27:4e679fecd547 181 pc.printf("\r\n Stato 1 - Start\r\n");
d3dfantasy99 27:4e679fecd547 182 }
d3dfantasy99 27:4e679fecd547 183 else if (buffer[0] == '2'){
d3dfantasy99 27:4e679fecd547 184 stato = 2;
d3dfantasy99 27:4e679fecd547 185 pc.printf("\r\n Stato 1 - Start\r\n");
d3dfantasy99 27:4e679fecd547 186 }
d3dfantasy99 27:4e679fecd547 187 }
d3dfantasy99 27:4e679fecd547 188 }
d3dfantasy99 27:4e679fecd547 189 }
d3dfantasy99 27:4e679fecd547 190 }
d3dfantasy99 27:4e679fecd547 191 }
d3dfantasy99 27:4e679fecd547 192
d3dfantasy99 27:4e679fecd547 193 void misure_invio_dati(void const *args) {
d3dfantasy99 27:4e679fecd547 194
d3dfantasy99 27:4e679fecd547 195
d3dfantasy99 27:4e679fecd547 196
d3dfantasy99 27:4e679fecd547 197
d3dfantasy99 27:4e679fecd547 198 }
d3dfantasy99 27:4e679fecd547 199 void rotazione_passo_passo(void const *args) {
d3dfantasy99 27:4e679fecd547 200
d3dfantasy99 27:4e679fecd547 201 }
d3dfantasy99 27:4e679fecd547 202
d3dfantasy99 27:4e679fecd547 203
d3dfantasy99 27:4e679fecd547 204
d3dfantasy99 27:4e679fecd547 205 */
d3dfantasy99 27:4e679fecd547 206 int main() {
d3dfantasy99 27:4e679fecd547 207 #ifdef TARGET_STM32F429
Davidroid 23:073b26366d03 208 DevSPI dev_spi(D11, D12, D13);
d3dfantasy99 27:4e679fecd547 209 #else
Davidroid 3:fd280b953f77 210 DevSPI dev_spi(D11, D12, D3);
d3dfantasy99 27:4e679fecd547 211 #endif
Davidroid 0:5148e9486cf2 212
Davidroid 26:caec5f51abe8 213 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Davidroid 0:5148e9486cf2 214
Davidroid 1:9f1974b0960d 215 /* Building a list of motor control components. */
davide.aliprandi@st.com 24:d1f487cb02ba 216 L6470 **motors = x_nucleo_ihm02a1->get_components();
d3dfantasy99 27:4e679fecd547 217
davide.aliprandi@st.com 24:d1f487cb02ba 218 motors[0]->set_home();
Davidroid 1:9f1974b0960d 219
Davidroid 1:9f1974b0960d 220 /* Waiting. */
Davidroid 1:9f1974b0960d 221 wait_ms(DELAY_1);
Davidroid 0:5148e9486cf2 222
Davidroid 1:9f1974b0960d 223 /* Getting the current position. */
davide.aliprandi@st.com 24:d1f487cb02ba 224 int position = motors[0]->get_position();
Davidroid 18:591a007effc2 225
Davidroid 1:9f1974b0960d 226 /* Printing to the console. */
Davidroid 1:9f1974b0960d 227 printf("--> Getting the current position: %d\r\n", position);
Davidroid 1:9f1974b0960d 228
Davidroid 1:9f1974b0960d 229 /* Waiting. */
Davidroid 1:9f1974b0960d 230 wait_ms(DELAY_1);
Davidroid 1:9f1974b0960d 231
Davidroid 1:9f1974b0960d 232 /* Printing to the console. */
Davidroid 1:9f1974b0960d 233 printf("--> Moving forward %d steps.\r\n", STEPS_1);
Davidroid 1:9f1974b0960d 234
Davidroid 1:9f1974b0960d 235 /* Moving. */
davide.aliprandi@st.com 24:d1f487cb02ba 236 motors[0]->move(StepperMotor::FWD, STEPS_1);
Davidroid 0:5148e9486cf2 237
Davidroid 1:9f1974b0960d 238 /* Waiting while active. */
davide.aliprandi@st.com 24:d1f487cb02ba 239 motors[0]->wait_while_active();
Davidroid 1:9f1974b0960d 240
Davidroid 1:9f1974b0960d 241 /* Getting the current position. */
davide.aliprandi@st.com 24:d1f487cb02ba 242 position = motors[0]->get_position();
Davidroid 1:9f1974b0960d 243
Davidroid 1:9f1974b0960d 244 /* Printing to the console. */
Davidroid 1:9f1974b0960d 245 printf("--> Getting the current position: %d\r\n", position);
Davidroid 1:9f1974b0960d 246
Davidroid 1:9f1974b0960d 247 /* Printing to the console. */
Davidroid 1:9f1974b0960d 248 printf("--> Marking the current position.\r\n");
Davidroid 0:5148e9486cf2 249
Davidroid 1:9f1974b0960d 250 /* Marking the current position. */
davide.aliprandi@st.com 24:d1f487cb02ba 251 motors[0]->set_mark();
Davidroid 1:9f1974b0960d 252
Davidroid 1:9f1974b0960d 253 /* Waiting. */
Davidroid 1:9f1974b0960d 254 wait_ms(DELAY_1);
Davidroid 1:9f1974b0960d 255
Davidroid 1:9f1974b0960d 256
Davidroid 1:9f1974b0960d 257
d3dfantasy99 27:4e679fecd547 258 /*
d3dfantasy99 27:4e679fecd547 259 //inizializzo la connessione seriale
d3dfantasy99 27:4e679fecd547 260 //inizializzo thread wifi e buffer
d3dfantasy99 27:4e679fecd547 261 Thread th_wifi(controllo_stato_connessione_thread);
d3dfantasy99 27:4e679fecd547 262 Thread th_buffer (controllo_dati_entrata_thread);
d3dfantasy99 27:4e679fecd547 263 Thread th_data (misure_invio_dati);
d3dfantasy99 27:4e679fecd547 264 Thread th_passo (rotazione_passo_passo);
d3dfantasy99 27:4e679fecd547 265 //Switch sullo stato
d3dfantasy99 27:4e679fecd547 266 while(1) {
d3dfantasy99 27:4e679fecd547 267 switch(stato){
d3dfantasy99 27:4e679fecd547 268 case 0:
d3dfantasy99 27:4e679fecd547 269 destra.write(0);
d3dfantasy99 27:4e679fecd547 270 sinistra.write(0);
d3dfantasy99 27:4e679fecd547 271
d3dfantasy99 27:4e679fecd547 272 /*
d3dfantasy99 27:4e679fecd547 273 char buffer1[1];
d3dfantasy99 27:4e679fecd547 274 buffer1[0] = 'a';
d3dfantasy99 27:4e679fecd547 275 int counta = 0;
d3dfantasy99 27:4e679fecd547 276 pc.printf("\r\nSending Data\r\n");
d3dfantasy99 27:4e679fecd547 277 counta = sizeof buffer1;
d3dfantasy99 27:4e679fecd547 278 buffer1 [counta]='\0';
d3dfantasy99 27:4e679fecd547 279 counta = socket.send(buffer1, sizeof buffer1);
Davidroid 1:9f1974b0960d 280
d3dfantasy99 27:4e679fecd547 281 if(counta > 0)
d3dfantasy99 27:4e679fecd547 282 {
d3dfantasy99 27:4e679fecd547 283
d3dfantasy99 27:4e679fecd547 284 printf("%s\r\n", buffer1);
d3dfantasy99 27:4e679fecd547 285 }
d3dfantasy99 27:4e679fecd547 286 */
d3dfantasy99 27:4e679fecd547 287 /* break;
Davidroid 18:591a007effc2 288
d3dfantasy99 27:4e679fecd547 289 case 1: //start
d3dfantasy99 27:4e679fecd547 290
d3dfantasy99 27:4e679fecd547 291 motori.period_ms(60);
d3dfantasy99 27:4e679fecd547 292 motori.pulsewidth(0.05);
d3dfantasy99 27:4e679fecd547 293 while(stato == 1){
d3dfantasy99 27:4e679fecd547 294 sinistra.write(1);
d3dfantasy99 27:4e679fecd547 295 wait_ms(60);
d3dfantasy99 27:4e679fecd547 296 destra.write(1);
d3dfantasy99 27:4e679fecd547 297 wait_ms(1500);
d3dfantasy99 27:4e679fecd547 298 destra.write(0);
d3dfantasy99 27:4e679fecd547 299 wait_ms(60);
d3dfantasy99 27:4e679fecd547 300 sinistra.write(0);
d3dfantasy99 27:4e679fecd547 301 wait_ms(1000);
d3dfantasy99 27:4e679fecd547 302 }
d3dfantasy99 27:4e679fecd547 303 break;
d3dfantasy99 27:4e679fecd547 304
d3dfantasy99 27:4e679fecd547 305 case 2: //test servo e misure
d3dfantasy99 27:4e679fecd547 306 int temp = 0;
d3dfantasy99 27:4e679fecd547 307 while(stato == 2){
d3dfantasy99 27:4e679fecd547 308
d3dfantasy99 27:4e679fecd547 309 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 27:4e679fecd547 310 if(distanza_centrale > test_distanza + 5){
d3dfantasy99 27:4e679fecd547 311 if(temp < 100){
d3dfantasy99 27:4e679fecd547 312 for(temp = gradi_servo; temp<100; temp = temp + 10) {
d3dfantasy99 27:4e679fecd547 313 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 27:4e679fecd547 314 board->sensor_centre->GetDistance(&distanza_destra);
d3dfantasy99 27:4e679fecd547 315 board->sensor_centre->GetDistance(&distanza_sinistra);
d3dfantasy99 27:4e679fecd547 316 printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
d3dfantasy99 27:4e679fecd547 317 wait_ms(500);
d3dfantasy99 27:4e679fecd547 318 servomotore = temp/100.0;
d3dfantasy99 27:4e679fecd547 319 wait_ms(0.01);
d3dfantasy99 27:4e679fecd547 320 }
d3dfantasy99 27:4e679fecd547 321
d3dfantasy99 27:4e679fecd547 322 }
d3dfantasy99 27:4e679fecd547 323 else{
d3dfantasy99 27:4e679fecd547 324 for(temp = gradi_servo; temp<100; temp = temp + 10) {
d3dfantasy99 27:4e679fecd547 325 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 27:4e679fecd547 326 board->sensor_centre->GetDistance(&distanza_destra);
d3dfantasy99 27:4e679fecd547 327 board->sensor_centre->GetDistance(&distanza_sinistra);
d3dfantasy99 27:4e679fecd547 328 printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
d3dfantasy99 27:4e679fecd547 329 wait_ms(500);
d3dfantasy99 27:4e679fecd547 330 servomotore = temp/100.0;
d3dfantasy99 27:4e679fecd547 331 wait_ms(0.01);
d3dfantasy99 27:4e679fecd547 332 }
d3dfantasy99 27:4e679fecd547 333
d3dfantasy99 27:4e679fecd547 334
d3dfantasy99 27:4e679fecd547 335 }
Davidroid 22:e81ccf73bc5d 336 }
d3dfantasy99 27:4e679fecd547 337
d3dfantasy99 27:4e679fecd547 338 break;
d3dfantasy99 27:4e679fecd547 339
Davidroid 22:e81ccf73bc5d 340 }
d3dfantasy99 27:4e679fecd547 341 }