Revision 0:6bde2b4e342e, committed 2017-04-04

Comitter:

Giuliove

Date:

Tue Apr 04 09:25:16 2017 +0000

Commit message:

Inseguimento di una traiettoria dritta mediante misure fornite dal giroscopio

Changed in this revision

diff -r 000000000000 -r 6bde2b4e342e X-NUCLEO-IHM12A1.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X-NUCLEO-IHM12A1.lib	Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM12A1/#232e0c730f59

diff -r 000000000000 -r 6bde2b4e342e X_NUCLEO_IKS01A2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IKS01A2.lib	Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A2/#63b2b4c21092

diff -r 000000000000 -r 6bde2b4e342e main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,205 @@
+ /**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  Giulio Vespoli, Unina Fablab
+ * @version 1.0
+ * @date    04-April-2017
+ * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
+ *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
+ ******************************************************************************
+*/
+
+
+/* Includes */
+#include "mbed.h"
+#include "rtos.h"
+#include "x_nucleo_iks01a2.h"
+
+#define RAD_TO_DEG 57.295779513082320876798154814105f
+
+
+
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11); //d4 diventa d11 e d5 diventa d10
+
+static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
+static LSM303AGR_ACC_Sensor *accelerometer = mems_expansion_board->accelerometer;
+
+
+int32_t mdps[3];
+int16_t mdpsRaw[3];
+int32_t acc[3];
+
+
+bool useCalibrate = false;
+float dpsPerDigit = .07f; //70 mdps diventeranno 0.07dps
+float gyrosensitivity; 
+
+
+float offset=0;         
+float correct_speed=0;         
+float acquired_speed=0;
+float yaw = 0;
+
+
+
+void readRaw()
+{
+  acc_gyro->Get_G_AxesRaw(mdpsRaw);
+  acquired_speed = mdpsRaw[2];           //acquisisco velocità angolare dal giroscopio
+}
+
+
+
+float readNormalize()
+{
+    readRaw();
+    correct_speed = (acquired_speed - offset) * dpsPerDigit;       //tolgo al valore acquisito l'offset
+    return correct_speed;
+}
+
+
+  
+void calibrate(int samples)
+{
+    printf("CALIBRATING\r\n");
+ 
+    float sumZ = 0;   
+    for (uint8_t i = 0; i < samples; ++i)
+    {   
+        acc_gyro->Get_G_AxesRaw(mdpsRaw);                                               //Acquisisco dal giroscopio 
+       printf("SAMPLE - LSM6DSL [gyro/mdps]:   %6ld, %6ld,\r\n", i, mdpsRaw[2]);        //Stampo il numero del campione acquisito e la velocità angolare
+        sumZ += mdpsRaw[2];                                                             //Faccio la somma delle velocità angolari acquisite
+        wait(0.1);
+    }
+    
+    offset = sumZ / samples;                                                             //calcolo l'offset come valor medio di 100 acquisizioni
+    
+    printf("CALIBRATED\r\n");    
+    printf("\r\n");
+}
+
+
+
+void calcRotation2(void const *args)
+{
+    
+    Timer t;
+    double dt=0;                           //in dt memorizzerò l'intervallo di tempo tra due acquisizioni 
+    
+    while(1) 
+    {
+        t.stop();
+        dt= t.read();
+        
+        correct_speed = readNormalize();
+        t.reset();
+        t.start();
+        
+        // Calcolo lo yaw 
+        yaw = yaw + correct_speed * dt;    //lo spostamento complessivo al passo k sarà uguale allo spostamento complessivo all'istante k-1 + la nuova rotazione
+    }
+}
+      
+
+
+ 
+
+/* Component specific header files. */
+#include "stspin240_250_class.h"
+STSPIN240_250 *motor;
+
+
+int main()
+{ 
+  uint8_t id;
+  acc_gyro->Enable_X();   //ABILITO ACCELLEROMETRO
+  acc_gyro->Enable_G();   //ABILITO GIROSCOPIO
+  
+  accelerometer->Enable();  //ABILITO UN ALTRO ACCELLEROMETRO
+  
+  printf("\r\n--- Starting new run ---\r\n");
+ 
+  wait(1);
+  
+  acc_gyro->ReadID(&id);                                          //RICAVO L'INDIRIZZO DEL GIROSCOPIO E ACCELLEROMETRO
+  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+ 
+  accelerometer->ReadID(&id);                                       //RICAVO L'INDIRIZZO DELL'ACCELLEROMETRO
+  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
+  
+  
+   acc_gyro->Get_G_Sensitivity(&gyrosensitivity);                  //RICAVO LA SENSIBILITA' DEL GIRO E ACC, 70
+   printf("Sensitivity mV/o/ms:  %f\r\n", gyrosensitivity);
+   
+   float fullScale;
+   acc_gyro->Get_G_FS(&fullScale);                                
+   printf("Full Scale:  %f\r\n", fullScale);
+   
+   float gODR;
+   acc_gyro->Get_G_ODR(&gODR);
+   printf("ODR:  %f\r\n", gODR);
+    
+   calibrate(100);                                                   //Uso 100 campioni per calcolare l'offset
+   
+   Thread rotTrigger(calcRotation2);                                 //Chiamo la funzione calcRotation2 che aggiorna il valore dello yaw
+    
+ 
+ 
+ /* Initializing Motor Control Component. */
+    motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 );         //Chiamo il costruttore per inizializzare l'oggetto motor
+
+    /* Set dual bridge enabled as two motors are used*/
+    motor->SetDualFullBridgeConfig(1);
+
+    /* Set PWM Frequency of Ref to 20000 Hz */
+    motor->SetRefPwmFreq(0, 20000);       
+   motor->SetRefPwmFreq(1, 20000);
+
+    /* Set PWM duty cycle of Ref to 100% */
+    motor->SetRefPwmDc(0, 100);       
+
+    motor->SetRefPwmDc(1, 100);
+    
+    /* Set PWM Frequency of bridge A inputs to 20000 Hz */
+    motor->SetBridgeInputPwmFreq(0,20000);
+
+    /* Set PWM Frequency of bridge B inputs to 20000 Hz */
+    motor->SetBridgeInputPwmFreq(1,20000);
+               
+                
+
+                int speed_sx=35;
+                int speed_dx=35;
+                
+                
+                motor->SetSpeed(0,speed_sx);                       
+                motor->SetSpeed(1,speed_dx);                         
+                motor->Run(1, BDCMotor::FWD);                   
+                motor->Run(0, BDCMotor::FWD);    
+
+        while(1)
+        {
+            if(yaw>0.0001f)  //90° corrisponde ad uno yaw di 0.08, quindi 0.0001 corrisponde ad un decimo di grado (0.1°)
+            {
+                speed_sx=45;
+                speed_dx=35;
+            }
+            
+            if(yaw<-0.0001f)
+            {
+                speed_dx=45;
+                speed_sx=35;
+            }
+            
+               printf( "yaw = %2.4f \r\n", yaw);
+               motor->SetSpeed(0,speed_sx);   
+               motor->SetSpeed(1,speed_dx); 
+               printf( "VELOCITA SX = %6ld\r\n", speed_sx);
+               printf( "VELOCITA DX= %6ld\r\n", speed_dx);
+               printf("\r\n");
+               
+        }
+}
+
+ 

diff -r 000000000000 -r 6bde2b4e342e mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#12552ef4e980

diff -r 000000000000 -r 6bde2b4e342e mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/d75b3fe1f5cb
\ No newline at end of file