Progetto STM32 / Mbed 2 deprecated HelloWorld_IHM01A1

d

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

Fork of HelloWorld_IHM01A1 by ST

main.cpp@38:2fdc20bbb354, 2017-04-29 (annotated)

Committer:
d3dfantasy99
Date:
Sat Apr 29 06:34:44 2017 +0000
Revision:
38:2fdc20bbb354
Parent:
35:2b44ed4ec7a0 
c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
d3dfantasy99 38:2fdc20bbb354 1 // Importo Librerie
d3dfantasy99 38:2fdc20bbb354 2 #include "mbed.h"
d3dfantasy99 38:2fdc20bbb354 3 /*#include "hcsr04.h"
d3dfantasy99 38:2fdc20bbb354 4 #include "SpwfInterface.h"
d3dfantasy99 38:2fdc20bbb354 5 #include "TCPSocket.h"
d3dfantasy99 38:2fdc20bbb354 6 */#include "DevSPI.h"
d3dfantasy99 38:2fdc20bbb354 7 #include "L6474.h"
d3dfantasy99 38:2fdc20bbb354 8 /*#include "XNucleoIHM02A1.h"
d3dfantasy99 38:2fdc20bbb354 9 /*#include "x_nucleo_53l0a1.h"
d3dfantasy99 38:2fdc20bbb354 10 #include <stdio.h>
d3dfantasy99 38:2fdc20bbb354 11 #include "Servo.h"
d3dfantasy99 38:2fdc20bbb354 12 #include "rtos.h"*/
Davidroid 0:e6a49a092e2a 13
Davidroid 26:b0203c2265e5 14 #define STEPS_1 (400 * 8) /* 1 revolution given a 400 steps motor configured at 1/8 microstep mode. */
Davidroid 26:b0203c2265e5 15
Davidroid 26:b0203c2265e5 16 /* Delay in milliseconds. */
Davidroid 26:b0203c2265e5 17 #define DELAY_1 1000
Davidroid 26:b0203c2265e5 18 #define DELAY_2 2000
Davidroid 26:b0203c2265e5 19 #define DELAY_3 6000
Davidroid 26:b0203c2265e5 20 #define DELAY_4 8000
Davidroid 26:b0203c2265e5 21
Davidroid 29:a80a213c3c94 22 /* Speed in pps (Pulses Per Second).
Davidroid 29:a80a213c3c94 23 In Full Step mode: 1 pps = 1 step/s).
Davidroid 29:a80a213c3c94 24 In 1/N Step Mode: N pps = 1 step/s). */
Davidroid 26:b0203c2265e5 25 #define SPEED_1 2400
Davidroid 26:b0203c2265e5 26 #define SPEED_2 1200
Davidroid 0:e6a49a092e2a 27
Davidroid 0:e6a49a092e2a 28
Davidroid 0:e6a49a092e2a 29 /* Variables -----------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 30
Davidroid 19:1cd7f65c155c 31 /* Initialization parameters. */
davide.aliprandi@st.com 35:2b44ed4ec7a0 32 L6474_init_t init = {
Davidroid 29:a80a213c3c94 33 160, /* Acceleration rate in pps^2. Range: (0..+inf). */
Davidroid 29:a80a213c3c94 34 160, /* Deceleration rate in pps^2. Range: (0..+inf). */
Davidroid 29:a80a213c3c94 35 1600, /* Maximum speed in pps. Range: (30..10000]. */
Davidroid 29:a80a213c3c94 36 800, /* Minimum speed in pps. Range: [30..10000). */
Davidroid 19:1cd7f65c155c 37 250, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
Davidroid 19:1cd7f65c155c 38 L6474_OCD_TH_750mA, /* Overcurrent threshold (OCD_TH register). */
Davidroid 19:1cd7f65c155c 39 L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
Davidroid 19:1cd7f65c155c 40 L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
Davidroid 26:b0203c2265e5 41 L6474_STEP_SEL_1_8, /* Step selection (STEP_SEL field of STEP_MODE register). */
Davidroid 19:1cd7f65c155c 42 L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
Davidroid 19:1cd7f65c155c 43 L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
Davidroid 19:1cd7f65c155c 44 L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
Davidroid 19:1cd7f65c155c 45 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
Davidroid 19:1cd7f65c155c 46 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
Davidroid 19:1cd7f65c155c 47 L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
Davidroid 19:1cd7f65c155c 48 L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
Davidroid 19:1cd7f65c155c 49 L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
Davidroid 19:1cd7f65c155c 50 L6474_ALARM_EN_OVERCURRENT |
Davidroid 19:1cd7f65c155c 51 L6474_ALARM_EN_THERMAL_SHUTDOWN |
Davidroid 19:1cd7f65c155c 52 L6474_ALARM_EN_THERMAL_WARNING |
Davidroid 19:1cd7f65c155c 53 L6474_ALARM_EN_UNDERVOLTAGE |
Davidroid 19:1cd7f65c155c 54 L6474_ALARM_EN_SW_TURN_ON |
Davidroid 19:1cd7f65c155c 55 L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
Davidroid 19:1cd7f65c155c 56 };
Davidroid 19:1cd7f65c155c 57
Davidroid 0:e6a49a092e2a 58 /* Motor Control Component. */
d3dfantasy99 38:2fdc20bbb354 59
Davidroid 0:e6a49a092e2a 60
Davidroid 0:e6a49a092e2a 61
Davidroid 19:1cd7f65c155c 62 /* Functions -----------------------------------------------------------------*/
Davidroid 19:1cd7f65c155c 63
Davidroid 19:1cd7f65c155c 64 /**
Davidroid 25:3c863b420ac5 65 * @brief This is an example of user handler for the flag interrupt.
Davidroid 25:3c863b420ac5 66 * @param None
Davidroid 25:3c863b420ac5 67 * @retval None
Davidroid 25:3c863b420ac5 68 * @note If needed, implement it, and then attach and enable it:
davide.aliprandi@st.com 35:2b44ed4ec7a0 69 * + motor->attach_flag_irq(&flag_irq_handler);
davide.aliprandi@st.com 35:2b44ed4ec7a0 70 * + motor->enable_flag_irq();
Davidroid 25:3c863b420ac5 71 * To disable it:
davide.aliprandi@st.com 35:2b44ed4ec7a0 72 * + motor->disble_flag_irq();
Davidroid 25:3c863b420ac5 73 */
davide.aliprandi@st.com 35:2b44ed4ec7a0 74 void flag_irq_handler(void)
Davidroid 19:1cd7f65c155c 75 {
Davidroid 19:1cd7f65c155c 76 /* Set ISR flag. */
Davidroid 19:1cd7f65c155c 77 motor->isr_flag = TRUE;
Davidroid 19:1cd7f65c155c 78
Davidroid 19:1cd7f65c155c 79 /* Get the value of the status register. */
davide.aliprandi@st.com 35:2b44ed4ec7a0 80 unsigned int status = motor->get_status();
Davidroid 34:543d0d1147d9 81
Davidroid 19:1cd7f65c155c 82 /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
Davidroid 19:1cd7f65c155c 83 /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
Davidroid 34:543d0d1147d9 84 if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
Davidroid 19:1cd7f65c155c 85 printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
Davidroid 34:543d0d1147d9 86 }
Davidroid 19:1cd7f65c155c 87
Davidroid 19:1cd7f65c155c 88 /* Reset ISR flag. */
Davidroid 19:1cd7f65c155c 89 motor->isr_flag = FALSE;
Davidroid 19:1cd7f65c155c 90 }
Davidroid 19:1cd7f65c155c 91
d3dfantasy99 38:2fdc20bbb354 92 /*
d3dfantasy99 38:2fdc20bbb354 93 #define VL53L0_I2C_SDA D14 //sensore fotoni
d3dfantasy99 38:2fdc20bbb354 94 #define VL53L0_I2C_SCL D15 //sensore fotoni
d3dfantasy99 38:2fdc20bbb354 95 */
Davidroid 19:1cd7f65c155c 96
d3dfantasy99 38:2fdc20bbb354 97
d3dfantasy99 38:2fdc20bbb354 98
d3dfantasy99 38:2fdc20bbb354 99
d3dfantasy99 38:2fdc20bbb354 100
d3dfantasy99 38:2fdc20bbb354 101
d3dfantasy99 38:2fdc20bbb354 102
d3dfantasy99 38:2fdc20bbb354 103
d3dfantasy99 38:2fdc20bbb354 104 /*DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
d3dfantasy99 38:2fdc20bbb354 105 static X_NUCLEO_53L0A1 *board=NULL; //sensore fotoni*/
d3dfantasy99 38:2fdc20bbb354 106 /*
d3dfantasy99 38:2fdc20bbb354 107 //Inizializzo le schede
d3dfantasy99 38:2fdc20bbb354 108 Serial pc(USBTX, USBRX); //seriale
d3dfantasy99 38:2fdc20bbb354 109 SpwfSAInterface wifi(D8, D2, false); //wifi
d3dfantasy99 38:2fdc20bbb354 110 Servo servomotore(PB_10); //servomotore
d3dfantasy99 38:2fdc20bbb354 111 PwmOut motori(PA_8);//D13
d3dfantasy99 38:2fdc20bbb354 112 DigitalOut sinistra(PA_0);
d3dfantasy99 38:2fdc20bbb354 113 DigitalOut destra(PA_1);
d3dfantasy99 38:2fdc20bbb354 114 TCPSocket socket(&wifi);
d3dfantasy99 38:2fdc20bbb354 115
d3dfantasy99 38:2fdc20bbb354 116
d3dfantasy99 38:2fdc20bbb354 117 //Variabili Globali
d3dfantasy99 38:2fdc20bbb354 118 char * ssid = "TekSmartLab"; //ssid wifi
d3dfantasy99 38:2fdc20bbb354 119 char * seckey = ""; //password wifi
d3dfantasy99 38:2fdc20bbb354 120 char * ip_socket = "192.168.1.104"; //ip socket master
d3dfantasy99 38:2fdc20bbb354 121 int porta_socket = 8000; //porta del socket
d3dfantasy99 38:2fdc20bbb354 122 int stato_socket = 0;
d3dfantasy99 38:2fdc20bbb354 123 int stato = 0; // 1 = start - 0 = stop (default stop)
d3dfantasy99 38:2fdc20bbb354 124 int gradi_servo = 45;
d3dfantasy99 38:2fdc20bbb354 125 char buffer[0];
d3dfantasy99 38:2fdc20bbb354 126 int controllo_buffer_in_arrivo = 0;
d3dfantasy99 38:2fdc20bbb354 127 int contatore_buff = 0;
d3dfantasy99 38:2fdc20bbb354 128 int invio_dati = 0;
d3dfantasy99 38:2fdc20bbb354 129 int buca_trovata = 0;
d3dfantasy99 38:2fdc20bbb354 130 int start_servo = 0;
d3dfantasy99 38:2fdc20bbb354 131 uint32_t distanza_centrale = 0;
d3dfantasy99 38:2fdc20bbb354 132 uint32_t distanza_destra = 0;
d3dfantasy99 38:2fdc20bbb354 133 uint32_t distanza_sinistra = 0;
d3dfantasy99 38:2fdc20bbb354 134 uint32_t test_distanza = 0;
d3dfantasy99 38:2fdc20bbb354 135 int misure[2];*/
d3dfantasy99 38:2fdc20bbb354 136
d3dfantasy99 38:2fdc20bbb354 137 /* Second Motor. */
d3dfantasy99 38:2fdc20bbb354 138
d3dfantasy99 38:2fdc20bbb354 139
d3dfantasy99 38:2fdc20bbb354 140
d3dfantasy99 38:2fdc20bbb354 141 /*
d3dfantasy99 38:2fdc20bbb354 142
d3dfantasy99 38:2fdc20bbb354 143 void controllo_stato_connessione_thread(void const *args) {
d3dfantasy99 38:2fdc20bbb354 144 while(1){
d3dfantasy99 38:2fdc20bbb354 145 while(stato_socket == 0) {
d3dfantasy99 38:2fdc20bbb354 146
Davidroid 0:e6a49a092e2a 147
d3dfantasy99 38:2fdc20bbb354 148 /* Initializing Motor Control Component. */
d3dfantasy99 38:2fdc20bbb354 149
d3dfantasy99 38:2fdc20bbb354 150
d3dfantasy99 38:2fdc20bbb354 151 /* Attaching and enabling interrupt handlers. */
d3dfantasy99 38:2fdc20bbb354 152
d3dfantasy99 38:2fdc20bbb354 153 /*
d3dfantasy99 38:2fdc20bbb354 154 board=X_NUCLEO_53L0A1::Instance(device_i2c, A2, D10, D9);
d3dfantasy99 38:2fdc20bbb354 155 board->InitBoard();
d3dfantasy99 38:2fdc20bbb354 156 DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
d3dfantasy99 38:2fdc20bbb354 157 board->sensor_centre->GetDistance(&test_distanza);
d3dfantasy99 38:2fdc20bbb354 158
d3dfantasy99 38:2fdc20bbb354 159 servomotore = gradi_servo/100.0;
d3dfantasy99 38:2fdc20bbb354 160 destra.write(0);
d3dfantasy99 38:2fdc20bbb354 161 sinistra.write(0);
d3dfantasy99 38:2fdc20bbb354 162 pc.printf("\r\n Rift Searcher 1.0 \r\n");
d3dfantasy99 38:2fdc20bbb354 163 /* while(!wifi.connect(ssid, seckey, NSAPI_SECURITY_NONE))/*NSAPI_SECURITY_WPA2*/ //{
d3dfantasy99 38:2fdc20bbb354 164 /* pc.printf("\r\n Connessione alla rete wifi corso... Attendere...\r\n");
d3dfantasy99 38:2fdc20bbb354 165 wait_ms(2000);
d3dfantasy99 38:2fdc20bbb354 166 }
d3dfantasy99 38:2fdc20bbb354 167 //connesso alla rete wifi
d3dfantasy99 38:2fdc20bbb354 168 pc.printf("\r\n Connesso alla rete Wifi\r\n");
d3dfantasy99 38:2fdc20bbb354 169
d3dfantasy99 38:2fdc20bbb354 170 //verifico la connessione al socket
d3dfantasy99 38:2fdc20bbb354 171
d3dfantasy99 38:2fdc20bbb354 172 while(socket.connect(ip_socket, porta_socket) != 0){
d3dfantasy99 38:2fdc20bbb354 173 pc.printf("\r\n Connessione al socket corso... Attendere...\r\n");
d3dfantasy99 38:2fdc20bbb354 174 wait_ms(2000);
d3dfantasy99 38:2fdc20bbb354 175 }
d3dfantasy99 38:2fdc20bbb354 176 //connesso al socket
d3dfantasy99 38:2fdc20bbb354 177 pc.printf("\r\n Connessione al socket riuscita\r\n");
d3dfantasy99 38:2fdc20bbb354 178 stato_socket = 1;
d3dfantasy99 38:2fdc20bbb354 179 }
d3dfantasy99 38:2fdc20bbb354 180 }
d3dfantasy99 38:2fdc20bbb354 181 }
Davidroid 6:32166bfc04b0 182
d3dfantasy99 38:2fdc20bbb354 183 void controllo_dati_entrata_thread(void const *args) {
d3dfantasy99 38:2fdc20bbb354 184 pc.printf("\r\n Controllo dati\r\n");
d3dfantasy99 38:2fdc20bbb354 185 while(1){
d3dfantasy99 38:2fdc20bbb354 186 if(stato_socket == 1){
d3dfantasy99 38:2fdc20bbb354 187 char buffer[1];
d3dfantasy99 38:2fdc20bbb354 188 if(socket.recv(buffer, sizeof buffer) != 0){
d3dfantasy99 38:2fdc20bbb354 189 int count = 0;
d3dfantasy99 38:2fdc20bbb354 190 count = socket.recv(buffer, sizeof buffer);
d3dfantasy99 38:2fdc20bbb354 191 if(count > 0){
d3dfantasy99 38:2fdc20bbb354 192 buffer [count]='\0';
d3dfantasy99 38:2fdc20bbb354 193 if(buffer[0] == '0'){
d3dfantasy99 38:2fdc20bbb354 194 stato = 0;
d3dfantasy99 38:2fdc20bbb354 195 pc.printf("\r\n Stato 0\r\n");
d3dfantasy99 38:2fdc20bbb354 196 }
d3dfantasy99 38:2fdc20bbb354 197 else if (buffer[0] == '1'){
d3dfantasy99 38:2fdc20bbb354 198 stato = 1;
d3dfantasy99 38:2fdc20bbb354 199 pc.printf("\r\n Stato 1 - Start\r\n");
d3dfantasy99 38:2fdc20bbb354 200 }
d3dfantasy99 38:2fdc20bbb354 201 else if (buffer[0] == '2'){
d3dfantasy99 38:2fdc20bbb354 202 stato = 2;
d3dfantasy99 38:2fdc20bbb354 203 pc.printf("\r\n Stato 1 - Start\r\n");
d3dfantasy99 38:2fdc20bbb354 204 }
d3dfantasy99 38:2fdc20bbb354 205 }
d3dfantasy99 38:2fdc20bbb354 206 }
d3dfantasy99 38:2fdc20bbb354 207 }
d3dfantasy99 38:2fdc20bbb354 208 }
d3dfantasy99 38:2fdc20bbb354 209 }
d3dfantasy99 38:2fdc20bbb354 210
d3dfantasy99 38:2fdc20bbb354 211 void misure_invio_dati(void const *args) {
d3dfantasy99 38:2fdc20bbb354 212
d3dfantasy99 38:2fdc20bbb354 213
d3dfantasy99 38:2fdc20bbb354 214
d3dfantasy99 38:2fdc20bbb354 215
d3dfantasy99 38:2fdc20bbb354 216 }
d3dfantasy99 38:2fdc20bbb354 217 void rotazione_passo_passo(void const *args) {
d3dfantasy99 38:2fdc20bbb354 218
d3dfantasy99 38:2fdc20bbb354 219 }
d3dfantasy99 38:2fdc20bbb354 220
d3dfantasy99 38:2fdc20bbb354 221
d3dfantasy99 38:2fdc20bbb354 222
d3dfantasy99 38:2fdc20bbb354 223 */
d3dfantasy99 38:2fdc20bbb354 224
d3dfantasy99 38:2fdc20bbb354 225
d3dfantasy99 38:2fdc20bbb354 226 /* Attaching and enabling interrupt handlers. */
d3dfantasy99 38:2fdc20bbb354 227
d3dfantasy99 38:2fdc20bbb354 228
d3dfantasy99 38:2fdc20bbb354 229 int main() {
d3dfantasy99 38:2fdc20bbb354 230
Davidroid 0:e6a49a092e2a 231 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 232
Davidroid 11:3e303a25770d 233 /* Initializing Motor Control Component. */
Davidroid 5:8065b587ade0 234 motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
davide.aliprandi@st.com 35:2b44ed4ec7a0 235 if (motor->init(&init) != COMPONENT_OK) {
Davidroid 17:4830b25fec7f 236 exit(EXIT_FAILURE);
Davidroid 34:543d0d1147d9 237 }
d3dfantasy99 38:2fdc20bbb354 238 motor->attach_flag_irq(&flag_irq_handler);
davide.aliprandi@st.com 35:2b44ed4ec7a0 239 motor->enable_flag_irq();
Davidroid 0:e6a49a092e2a 240 /* Printing to the console. */
Davidroid 0:e6a49a092e2a 241 printf("Motor Control Application Example for 1 Motor\r\n\n");
Davidroid 0:e6a49a092e2a 242
Davidroid 6:32166bfc04b0 243
Davidroid 6:32166bfc04b0 244 /*----- Moving. -----*/
Davidroid 6:32166bfc04b0 245
Davidroid 6:32166bfc04b0 246 /* Printing to the console. */
Davidroid 26:b0203c2265e5 247 printf("--> Moving forward %d steps.\r\n", STEPS_1);
Davidroid 6:32166bfc04b0 248
Davidroid 6:32166bfc04b0 249 /* Moving N steps in the forward direction. */
davide.aliprandi@st.com 35:2b44ed4ec7a0 250 motor->move(StepperMotor::FWD, STEPS_1);
Davidroid 6:32166bfc04b0 251
Davidroid 6:32166bfc04b0 252 /* Waiting while the motor is active. */
davide.aliprandi@st.com 35:2b44ed4ec7a0 253 motor->wait_while_active();
Davidroid 6:32166bfc04b0 254
Davidroid 6:32166bfc04b0 255 /* Getting current position. */
davide.aliprandi@st.com 35:2b44ed4ec7a0 256 int position = motor->get_position();
Davidroid 6:32166bfc04b0 257
d3dfantasy99 38:2fdc20bbb354 258 /*
d3dfantasy99 38:2fdc20bbb354 259 //inizializzo la connessione seriale
d3dfantasy99 38:2fdc20bbb354 260 //inizializzo thread wifi e buffer
d3dfantasy99 38:2fdc20bbb354 261 Thread th_wifi(controllo_stato_connessione_thread);
d3dfantasy99 38:2fdc20bbb354 262 Thread th_buffer (controllo_dati_entrata_thread);
d3dfantasy99 38:2fdc20bbb354 263 Thread th_data (misure_invio_dati);
d3dfantasy99 38:2fdc20bbb354 264 Thread th_passo (rotazione_passo_passo);
d3dfantasy99 38:2fdc20bbb354 265 //Switch sullo stato
d3dfantasy99 38:2fdc20bbb354 266 while(1) {
d3dfantasy99 38:2fdc20bbb354 267 switch(stato){
d3dfantasy99 38:2fdc20bbb354 268 case 0:
d3dfantasy99 38:2fdc20bbb354 269 destra.write(0);
d3dfantasy99 38:2fdc20bbb354 270 sinistra.write(0);
d3dfantasy99 38:2fdc20bbb354 271
d3dfantasy99 38:2fdc20bbb354 272 /*
d3dfantasy99 38:2fdc20bbb354 273 char buffer1[1];
d3dfantasy99 38:2fdc20bbb354 274 buffer1[0] = 'a';
d3dfantasy99 38:2fdc20bbb354 275 int counta = 0;
d3dfantasy99 38:2fdc20bbb354 276 pc.printf("\r\nSending Data\r\n");
d3dfantasy99 38:2fdc20bbb354 277 counta = sizeof buffer1;
d3dfantasy99 38:2fdc20bbb354 278 buffer1 [counta]='\0';
d3dfantasy99 38:2fdc20bbb354 279 counta = socket.send(buffer1, sizeof buffer1);
Davidroid 6:32166bfc04b0 280
d3dfantasy99 38:2fdc20bbb354 281 if(counta > 0)
d3dfantasy99 38:2fdc20bbb354 282 {
d3dfantasy99 38:2fdc20bbb354 283
d3dfantasy99 38:2fdc20bbb354 284 printf("%s\r\n", buffer1);
d3dfantasy99 38:2fdc20bbb354 285 }
d3dfantasy99 38:2fdc20bbb354 286 */
d3dfantasy99 38:2fdc20bbb354 287 /* break;
Davidroid 6:32166bfc04b0 288
d3dfantasy99 38:2fdc20bbb354 289 case 1: //start
d3dfantasy99 38:2fdc20bbb354 290
d3dfantasy99 38:2fdc20bbb354 291 motori.period_ms(60);
d3dfantasy99 38:2fdc20bbb354 292 motori.pulsewidth(0.05);
d3dfantasy99 38:2fdc20bbb354 293 while(stato == 1){
d3dfantasy99 38:2fdc20bbb354 294 sinistra.write(1);
d3dfantasy99 38:2fdc20bbb354 295 wait_ms(60);
d3dfantasy99 38:2fdc20bbb354 296 destra.write(1);
d3dfantasy99 38:2fdc20bbb354 297 wait_ms(1500);
d3dfantasy99 38:2fdc20bbb354 298 destra.write(0);
d3dfantasy99 38:2fdc20bbb354 299 wait_ms(60);
d3dfantasy99 38:2fdc20bbb354 300 sinistra.write(0);
d3dfantasy99 38:2fdc20bbb354 301 wait_ms(1000);
d3dfantasy99 38:2fdc20bbb354 302 }
d3dfantasy99 38:2fdc20bbb354 303 break;
d3dfantasy99 38:2fdc20bbb354 304
d3dfantasy99 38:2fdc20bbb354 305 case 2: //test servo e misure
d3dfantasy99 38:2fdc20bbb354 306 int temp = 0;
d3dfantasy99 38:2fdc20bbb354 307 while(stato == 2){
d3dfantasy99 38:2fdc20bbb354 308
d3dfantasy99 38:2fdc20bbb354 309 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 38:2fdc20bbb354 310 if(distanza_centrale > test_distanza + 5){
d3dfantasy99 38:2fdc20bbb354 311 if(temp < 100){
d3dfantasy99 38:2fdc20bbb354 312 for(temp = gradi_servo; temp<100; temp = temp + 10) {
d3dfantasy99 38:2fdc20bbb354 313 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 38:2fdc20bbb354 314 board->sensor_centre->GetDistance(&distanza_destra);
d3dfantasy99 38:2fdc20bbb354 315 board->sensor_centre->GetDistance(&distanza_sinistra);
d3dfantasy99 38:2fdc20bbb354 316 printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
d3dfantasy99 38:2fdc20bbb354 317 wait_ms(500);
d3dfantasy99 38:2fdc20bbb354 318 servomotore = temp/100.0;
d3dfantasy99 38:2fdc20bbb354 319 wait_ms(0.01);
d3dfantasy99 38:2fdc20bbb354 320 }
d3dfantasy99 38:2fdc20bbb354 321
d3dfantasy99 38:2fdc20bbb354 322 }
d3dfantasy99 38:2fdc20bbb354 323 else{
d3dfantasy99 38:2fdc20bbb354 324 for(temp = gradi_servo; temp<100; temp = temp + 10) {
d3dfantasy99 38:2fdc20bbb354 325 board->sensor_centre->GetDistance(&distanza_centrale);
d3dfantasy99 38:2fdc20bbb354 326 board->sensor_centre->GetDistance(&distanza_destra);
d3dfantasy99 38:2fdc20bbb354 327 board->sensor_centre->GetDistance(&distanza_sinistra);
d3dfantasy99 38:2fdc20bbb354 328 printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
d3dfantasy99 38:2fdc20bbb354 329 wait_ms(500);
d3dfantasy99 38:2fdc20bbb354 330 servomotore = temp/100.0;
d3dfantasy99 38:2fdc20bbb354 331 wait_ms(0.01);
d3dfantasy99 38:2fdc20bbb354 332 }
d3dfantasy99 38:2fdc20bbb354 333
d3dfantasy99 38:2fdc20bbb354 334
d3dfantasy99 38:2fdc20bbb354 335 }
Davidroid 0:e6a49a092e2a 336 }
d3dfantasy99 38:2fdc20bbb354 337
d3dfantasy99 38:2fdc20bbb354 338 break;
d3dfantasy99 38:2fdc20bbb354 339
d3dfantasy99 38:2fdc20bbb354 340 }*/
d3dfantasy99 38:2fdc20bbb354 341 }