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 "L6474.h"
00008 /*#include "XNucleoIHM02A1.h"
00009 /*#include "x_nucleo_53l0a1.h"
00010 #include <stdio.h>
00011 #include "Servo.h"
00012 #include "rtos.h"*/
00013 
00014 #define STEPS_1 (400 * 8)   /* 1 revolution given a 400 steps motor configured at 1/8 microstep mode. */
00015 
00016 /* Delay in milliseconds. */
00017 #define DELAY_1 1000
00018 #define DELAY_2 2000
00019 #define DELAY_3 6000
00020 #define DELAY_4 8000
00021 
00022 /* Speed in pps (Pulses Per Second).
00023    In Full Step mode: 1 pps = 1 step/s).
00024    In 1/N Step Mode:  N pps = 1 step/s). */
00025 #define SPEED_1 2400
00026 #define SPEED_2 1200
00027 
00028 
00029 /* Variables -----------------------------------------------------------------*/
00030 
00031 /* Initialization parameters. */
00032 L6474_init_t init = {
00033     160,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
00034     160,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
00035     1600,                             /* Maximum speed in pps. Range: (30..10000]. */
00036     800,                              /* Minimum speed in pps. Range: [30..10000). */
00037     250,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
00038     L6474_OCD_TH_750mA,               /* Overcurrent threshold (OCD_TH register). */
00039     L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
00040     L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
00041     L6474_STEP_SEL_1_8,               /* Step selection (STEP_SEL field of STEP_MODE register). */
00042     L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
00043     L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
00044     L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
00045     3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
00046     21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
00047     L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
00048     L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
00049     L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
00050     L6474_ALARM_EN_OVERCURRENT |
00051     L6474_ALARM_EN_THERMAL_SHUTDOWN |
00052     L6474_ALARM_EN_THERMAL_WARNING |
00053     L6474_ALARM_EN_UNDERVOLTAGE |
00054     L6474_ALARM_EN_SW_TURN_ON |
00055     L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
00056 };
00057 
00058 /* Motor Control Component. */
00059 
00060 
00061 
00062 /* Functions -----------------------------------------------------------------*/
00063 
00064 /**
00065  * @brief  This is an example of user handler for the flag interrupt.
00066  * @param  None
00067  * @retval None
00068  * @note   If needed, implement it, and then attach and enable it:
00069  *           + motor->attach_flag_irq(&flag_irq_handler);
00070  *           + motor->enable_flag_irq();
00071  *         To disable it:
00072  *           + motor->disble_flag_irq();
00073  */
00074 void flag_irq_handler(void)
00075 {
00076     /* Set ISR flag. */
00077     motor->isr_flag = TRUE;
00078 
00079     /* Get the value of the status register. */
00080     unsigned int status = motor->get_status();
00081 
00082     /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
00083     /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
00084     if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
00085         printf("    WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
00086     }
00087     
00088     /* Reset ISR flag. */
00089     motor->isr_flag = FALSE;
00090 }
00091 
00092 /*
00093 #define VL53L0_I2C_SDA   D14 //sensore fotoni
00094 #define VL53L0_I2C_SCL   D15 //sensore fotoni
00095 */
00096 
00097 
00098 
00099 
00100 
00101 
00102 
00103 
00104 /*DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00105 static X_NUCLEO_53L0A1 *board=NULL; //sensore fotoni*/
00106 /*
00107 //Inizializzo le schede
00108 Serial pc(USBTX, USBRX); //seriale
00109 SpwfSAInterface wifi(D8, D2, false); //wifi
00110 Servo servomotore(PB_10); //servomotore
00111 PwmOut motori(PA_8);//D13
00112 DigitalOut sinistra(PA_0);
00113 DigitalOut destra(PA_1);
00114 TCPSocket socket(&wifi);
00115 
00116 
00117 //Variabili Globali
00118 char * ssid = "TekSmartLab"; //ssid wifi
00119 char * seckey = ""; //password wifi
00120 char * ip_socket = "192.168.1.104"; //ip socket master
00121 int porta_socket = 8000; //porta del socket
00122 int stato_socket = 0;
00123 int stato = 0; // 1 = start - 0 = stop (default stop)
00124 int gradi_servo = 45;
00125 char buffer[0];
00126 int controllo_buffer_in_arrivo = 0;
00127 int contatore_buff = 0;
00128 int invio_dati = 0;
00129 int buca_trovata = 0;
00130 int start_servo = 0;
00131 uint32_t distanza_centrale = 0;
00132 uint32_t distanza_destra = 0;
00133 uint32_t distanza_sinistra = 0;
00134 uint32_t test_distanza = 0;
00135 int misure[2];*/
00136 
00137     /* Second Motor. */
00138 
00139    
00140 
00141 /*
00142       
00143 void controllo_stato_connessione_thread(void const *args) {
00144     while(1){
00145     while(stato_socket == 0) {
00146         
00147 
00148     /* Initializing Motor Control Component. */
00149    
00150     
00151     /* Attaching and enabling interrupt handlers. */
00152     
00153 /*
00154         board=X_NUCLEO_53L0A1::Instance(device_i2c, A2, D10, D9);
00155         board->InitBoard();
00156         DevI2C *device_i2c =new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00157         board->sensor_centre->GetDistance(&test_distanza);
00158         
00159         servomotore = gradi_servo/100.0;
00160         destra.write(0);
00161         sinistra.write(0);
00162         pc.printf("\r\n Rift Searcher 1.0 \r\n");
00163   /*      while(!wifi.connect(ssid, seckey, NSAPI_SECURITY_NONE))/*NSAPI_SECURITY_WPA2*/ //{      
00164     /*        pc.printf("\r\n Connessione alla rete wifi corso... Attendere...\r\n");
00165             wait_ms(2000);    
00166         }
00167         //connesso alla rete wifi   
00168         pc.printf("\r\n Connesso alla rete Wifi\r\n");
00169     
00170         //verifico la connessione al socket
00171         
00172         while(socket.connect(ip_socket, porta_socket) != 0){
00173             pc.printf("\r\n Connessione al socket corso... Attendere...\r\n");
00174             wait_ms(2000);
00175         }
00176         //connesso al socket
00177         pc.printf("\r\n Connessione al socket riuscita\r\n");
00178         stato_socket = 1;
00179         }    
00180     }
00181 }
00182 
00183 void controllo_dati_entrata_thread(void const *args) {
00184     pc.printf("\r\n Controllo dati\r\n");
00185     while(1){
00186     if(stato_socket == 1){
00187         char buffer[1];
00188         if(socket.recv(buffer, sizeof buffer) != 0){
00189             int count = 0;
00190             count = socket.recv(buffer, sizeof buffer);
00191             if(count > 0){
00192                 buffer [count]='\0';
00193                 if(buffer[0] == '0'){
00194                     stato = 0;
00195                     pc.printf("\r\n Stato 0\r\n");
00196                     }
00197                 else if (buffer[0] == '1'){
00198                     stato = 1;
00199                     pc.printf("\r\n Stato 1 - Start\r\n");
00200                     }
00201                  else if (buffer[0] == '2'){
00202                     stato = 2;
00203                     pc.printf("\r\n Stato 1 - Start\r\n");
00204                     }          
00205                 } 
00206             }
00207         }
00208     }
00209 }
00210 
00211 void misure_invio_dati(void const *args) {
00212         
00213         
00214             
00215                         
00216 }
00217 void rotazione_passo_passo(void const *args) {
00218     
00219 }
00220 
00221 
00222 
00223 */
00224  
00225 
00226     /* Attaching and enabling interrupt handlers. */
00227     
00228 
00229 int main() {
00230     
00231     DevSPI dev_spi(D11, D12, D13);
00232 
00233     /* Initializing Motor Control Component. */
00234     motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
00235     if (motor->init(&init) != COMPONENT_OK) {
00236         exit(EXIT_FAILURE);
00237     }
00238    motor->attach_flag_irq(&flag_irq_handler);
00239     motor->enable_flag_irq();
00240     /* Printing to the console. */
00241     printf("Motor Control Application Example for 1 Motor\r\n\n");
00242 
00243 
00244     /*----- Moving. -----*/
00245 
00246     /* Printing to the console. */
00247     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00248 
00249     /* Moving N steps in the forward direction. */
00250     motor->move(StepperMotor::FWD, STEPS_1);
00251 
00252     /* Waiting while the motor is active. */
00253     motor->wait_while_active();
00254 
00255     /* Getting current position. */
00256     int position = motor->get_position();
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 }