main.cpp

00001 // Importo Librerie
00002 #include "mbed.h"
00003 /*#include "hcsr04.h"
00004 #include "SpwfInterface.h"
00005 #include "TCPSocket.h"
00006 */#include "DevSPI.h"
00007 /*#include "XNucleoIHM02A1.h"
00008 /*#include "x_nucleo_53l0a1.h"
00009 #include <stdio.h>
00010 #include "Servo.h"
00011 #include "rtos.h"*/
00012 
00013 #define MPR_1 4
00014 #define STEPS_1 (400 * 128) 
00015 #define DELAY_1 1000
00016 #define DELAY_2 2000
00017 #define DELAY_3 5000
00018 /*
00019 #define VL53L0_I2C_SDA   D14 //sensore fotoni
00020 #define VL53L0_I2C_SCL   D15 //sensore fotoni
00021 */
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00031 static X_NUCLEO_53L0A1 *board=NULL; //sensore fotoni
00032 /*
00033 //Inizializzo le schede
00034 Serial pc(USBTX, USBRX); //seriale
00035 SpwfSAInterface wifi(D8, D2, false); //wifi
00036 Servo servomotore(PB_10); //servomotore
00037 PwmOut motori(PA_8);//D13
00038 DigitalOut sinistra(PA_0);
00039 DigitalOut destra(PA_1);
00040 TCPSocket socket(&wifi);
00041 
00042 
00043 //Variabili Globali
00044 char * ssid = "TekSmartLab"; //ssid wifi
00045 char * seckey = ""; //password wifi
00046 char * ip_socket = "192.168.1.104"; //ip socket master
00047 int porta_socket = 8000; //porta del socket
00048 int stato_socket = 0;
00049 int stato = 0; // 1 = start - 0 = stop (default stop)
00050 int gradi_servo = 45;
00051 char buffer[0];
00052 int controllo_buffer_in_arrivo = 0;
00053 int contatore_buff = 0;
00054 int invio_dati = 0;
00055 int buca_trovata = 0;
00056 int start_servo = 0;
00057 uint32_t distanza_centrale = 0;
00058 uint32_t distanza_destra = 0;
00059 uint32_t distanza_sinistra = 0;
00060 uint32_t test_distanza = 0;
00061 int misure[2];*/
00062 XNucleoIHM02A1 *x_nucleo_ihm02a1;
00063 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00064     /* First Motor. */
00065     {
00066         9.0,                           /* Motor supply voltage in V. */
00067         400,                           /* Min number of steps per revolution for the motor. */
00068         1.7,                           /* Max motor phase voltage in A. */
00069         3.06,                          /* Max motor phase voltage in V. */
00070         300.0,                         /* Motor initial speed [step/s]. */
00071         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00072         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00073         992.0,                         /* Motor maximum speed [step/s]. */
00074         0.0,                           /* Motor minimum speed [step/s]. */
00075         602.7,                         /* Motor full-step speed threshold [step/s]. */
00076         3.06,                          /* Holding kval [V]. */
00077         3.06,                          /* Constant speed kval [V]. */
00078         3.06,                          /* Acceleration starting kval [V]. */
00079         3.06,                          /* Deceleration starting kval [V]. */
00080         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00081         392.1569e-6,                   /* Start slope [s/step]. */
00082         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00083         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00084         0,                             /* Thermal compensation factor (range [0, 15]). */
00085         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00086         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00087         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00088         0xFF,                          /* Alarm conditions enable. */
00089         0x2E88                         /* Ic configuration. */
00090     },
00091 
00092     /* Second Motor. */
00093     {
00094         9.0,                           /* Motor supply voltage in V. */
00095         400,                           /* Min number of steps per revolution for the motor. */
00096         1.7,                           /* Max motor phase voltage in A. */
00097         3.06,                          /* Max motor phase voltage in V. */
00098         300.0,                         /* Motor initial speed [step/s]. */
00099         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00100         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00101         992.0,                         /* Motor maximum speed [step/s]. */
00102         0.0,                           /* Motor minimum speed [step/s]. */
00103         602.7,                         /* Motor full-step speed threshold [step/s]. */
00104         3.06,                          /* Holding kval [V]. */
00105         3.06,                          /* Constant speed kval [V]. */
00106         3.06,                          /* Acceleration starting kval [V]. */
00107         3.06,                          /* Deceleration starting kval [V]. */
00108         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00109         392.1569e-6,                   /* Start slope [s/step]. */
00110         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00111         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00112         0,                             /* Thermal compensation factor (range [0, 15]). */
00113         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00114         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00115         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00116         0xFF,                          /* Alarm conditions enable. */
00117         0x2E88                         /* Ic configuration. */
00118     }
00119 };
00120 
00121    
00122 
00123 /*
00124       
00125 void controllo_stato_connessione_thread(void const *args) {
00126     while(1){
00127     while(stato_socket == 0) {
00128         
00129 
00130     /* Initializing Motor Control Component. */
00131    
00132     
00133     /* Attaching and enabling interrupt handlers. */
00134     
00135 /*
00136         board=X_NUCLEO_53L0A1::Instance(device_i2c, A2, D10, D9);
00137         board->InitBoard();
00138         DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00139         board->sensor_centre->GetDistance(&test_distanza);
00140         
00141         servomotore = gradi_servo/100.0;
00142         destra.write(0);
00143         sinistra.write(0);
00144         pc.printf("\r\n Rift Searcher 1.0 \r\n");
00145   /*      while(!wifi.connect(ssid, seckey, NSAPI_SECURITY_NONE))/*NSAPI_SECURITY_WPA2*/ {      
00146     /*        pc.printf("\r\n Connessione alla rete wifi corso... Attendere...\r\n");
00147             wait_ms(2000);    
00148         }
00149         //connesso alla rete wifi   
00150         pc.printf("\r\n Connesso alla rete Wifi\r\n");
00151     
00152         //verifico la connessione al socket
00153         
00154         while(socket.connect(ip_socket, porta_socket) != 0){
00155             pc.printf("\r\n Connessione al socket corso... Attendere...\r\n");
00156             wait_ms(2000);
00157         }
00158         //connesso al socket
00159         pc.printf("\r\n Connessione al socket riuscita\r\n");
00160         stato_socket = 1;
00161         }    
00162     }
00163 }
00164 
00165 void controllo_dati_entrata_thread(void const *args) {
00166     pc.printf("\r\n Controllo dati\r\n");
00167     while(1){
00168     if(stato_socket == 1){
00169         char buffer[1];
00170         if(socket.recv(buffer, sizeof buffer) != 0){
00171             int count = 0;
00172             count = socket.recv(buffer, sizeof buffer);
00173             if(count > 0){
00174                 buffer [count]='\0';
00175                 if(buffer[0] == '0'){
00176                     stato = 0;
00177                     pc.printf("\r\n Stato 0\r\n");
00178                     }
00179                 else if (buffer[0] == '1'){
00180                     stato = 1;
00181                     pc.printf("\r\n Stato 1 - Start\r\n");
00182                     }
00183                  else if (buffer[0] == '2'){
00184                     stato = 2;
00185                     pc.printf("\r\n Stato 1 - Start\r\n");
00186                     }          
00187                 } 
00188             }
00189         }
00190     }
00191 }
00192 
00193 void misure_invio_dati(void const *args) {
00194         
00195         
00196             
00197                         
00198 }
00199 void rotazione_passo_passo(void const *args) {
00200     
00201 }
00202 
00203 
00204 
00205 */
00206 int main() {
00207     #ifdef TARGET_STM32F429
00208     DevSPI dev_spi(D11, D12, D13);
00209     #else
00210     DevSPI dev_spi(D11, D12, D3);
00211     #endif
00212 
00213     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00214 
00215     /* Building a list of motor control components. */
00216     L6470 **motors = x_nucleo_ihm02a1->get_components();
00217     
00218     motors[0]->set_home();
00219 
00220     /* Waiting. */
00221     wait_ms(DELAY_1);
00222 
00223     /* Getting the current position. */
00224     int position = motors[0]->get_position();
00225 
00226     /* Printing to the console. */
00227     printf("--> Getting the current position: %d\r\n", position);
00228 
00229     /* Waiting. */
00230     wait_ms(DELAY_1);
00231 
00232     /* Printing to the console. */
00233     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00234 
00235     /* Moving. */
00236     motors[0]->move(StepperMotor::FWD, STEPS_1);
00237 
00238     /* Waiting while active. */
00239     motors[0]->wait_while_active();
00240 
00241     /* Getting the current position. */
00242     position = motors[0]->get_position();
00243     
00244     /* Printing to the console. */
00245     printf("--> Getting the current position: %d\r\n", position);
00246 
00247     /* Printing to the console. */
00248     printf("--> Marking the current position.\r\n");
00249 
00250     /* Marking the current position. */
00251     motors[0]->set_mark();
00252 
00253     /* Waiting. */
00254     wait_ms(DELAY_1);
00255 
00256     
00257     
00258     /*
00259     //inizializzo la connessione seriale
00260     //inizializzo thread wifi e buffer
00261     Thread th_wifi(controllo_stato_connessione_thread);
00262     Thread th_buffer (controllo_dati_entrata_thread);
00263     Thread th_data (misure_invio_dati);
00264     Thread th_passo (rotazione_passo_passo);
00265     //Switch sullo stato
00266     while(1) {
00267        switch(stato){
00268         case 0:
00269          destra.write(0);
00270          sinistra.write(0);
00271          
00272     /*
00273     char buffer1[1];
00274     buffer1[0] = 'a';
00275     int counta = 0;
00276     pc.printf("\r\nSending Data\r\n"); 
00277     counta = sizeof buffer1;
00278     buffer1 [counta]='\0';
00279     counta = socket.send(buffer1, sizeof buffer1);
00280     
00281     if(counta > 0)
00282     {
00283         
00284         printf("%s\r\n", buffer1);  
00285     }    
00286 */
00287       /*  break;
00288     
00289         case 1: //start
00290         
00291         motori.period_ms(60);
00292         motori.pulsewidth(0.05);
00293         while(stato == 1){
00294             sinistra.write(1);
00295             wait_ms(60);
00296             destra.write(1);
00297             wait_ms(1500); 
00298             destra.write(0);
00299             wait_ms(60);
00300             sinistra.write(0);
00301             wait_ms(1000);
00302         }
00303         break;
00304         
00305         case 2: //test servo e misure
00306         int temp = 0;
00307         while(stato == 2){
00308             
00309             board->sensor_centre->GetDistance(&distanza_centrale);
00310                 if(distanza_centrale > test_distanza + 5){
00311                     if(temp < 100){
00312                     for(temp = gradi_servo; temp<100; temp = temp + 10) {        
00313                     board->sensor_centre->GetDistance(&distanza_centrale);
00314                     board->sensor_centre->GetDistance(&distanza_destra);
00315                     board->sensor_centre->GetDistance(&distanza_sinistra);
00316                     printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
00317                     wait_ms(500);
00318                     servomotore = temp/100.0;
00319                     wait_ms(0.01);
00320                     }
00321                     
00322             }
00323             else{
00324                 for(temp = gradi_servo; temp<100; temp = temp + 10) {        
00325                     board->sensor_centre->GetDistance(&distanza_centrale);
00326                     board->sensor_centre->GetDistance(&distanza_destra);
00327                     board->sensor_centre->GetDistance(&distanza_sinistra);
00328                     printf("Distanza centrale : %ld\n Distanza destra : %ld\n Distanza sinistra : %ld\n", distanza_centrale,distanza_destra,distanza_sinistra);
00329                     wait_ms(500);
00330                     servomotore = temp/100.0;
00331                     wait_ms(0.01);
00332             }           
00333             
00334             
00335             }
00336     }
00337     
00338         break;
00339         
00340     }
00341 }