Acquisition avec le gyroscope + début code servomoteur (commande fonctionnelle mais interface à refaire)

CMakeLists.txt

@@ -9,11 +9,12 @@ test.c
 spi_nb.c
 gyro.c
 Temps.c
+Servomoteur.c
 )
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
 pico_add_extra_outputs(test)
-target_link_libraries(test pico_stdlib hardware_spi hardware_structs)
+target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs)
 
 add_custom_target(Flash
     DEPENDS test

Servomoteur.c

@@ -0,0 +1,20 @@
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "Servomoteur.h"
+
+#define SERVO1 0
+
+void Servomoteur_Init(void){
+    gpio_set_function(SERVO1, GPIO_FUNC_PWM);
+    uint slice_num = pwm_gpio_to_slice_num(SERVO1);
+    pwm_config pwm_servo = pwm_get_default_config();
+    pwm_config_set_clkdiv_int(&pwm_servo, 254);
+    pwm_config_set_phase_correct(&pwm_servo, false);
+    pwm_config_set_wrap(&pwm_servo, 9842);
+
+
+    pwm_init(slice_num, &pwm_servo, true);
+    pwm_set_chan_level(slice_num, 0, 984 );
+
+
+}

Servomoteur.h

@@ -0,0 +1 @@
+void Servomoteur_Init(void);

graph.gp

@@ -0,0 +1,15 @@
+set title "Observation des 3 axes\nT_{acq} : 5 ms"
+
+set xlabel "Temps (s)"
+set ylabel "Vitesse (°/s)"
+
+
+plot "./acq_5ms_env_50ms.csv" using 2:5 w l title "Vitesse X"
+pause -1
+plot "./acq_5ms_env_50ms.csv" using 2:6 w l title "Vitesse Y"
+pause -1
+plot "./acq_5ms_env_50ms.csv" using 2:7 w l title "Vitesse Z"
+pause -1
+
+
+

graph_enr01.gp

@@ -0,0 +1,15 @@
+set title "Observation des 3 axes\nT_{acq} : 5 ms"
+
+set xlabel "Temps (s)"
+set ylabel "Vitesse (°/s)"
+
+
+plot "./acq_5ms_env_50ms_enr_01.csv" using 1:2 w l title "Vitesse X"
+pause -1
+plot "./acq_5ms_env_50ms_enr_01.csv" using 1:3 w l title "Vitesse Y"
+pause -1
+plot "./acq_5ms_env_50ms_enr_01.csv" using 1:4 w l title "Vitesse Z"
+pause -1
+
+
+

graph_enr02.gp

@@ -0,0 +1,10 @@
+set title "Observation des 3 axes\nT_{acq} : 5 ms"
+
+set xlabel "Temps (s)"
+set ylabel "Vitesse (°/s)"
+set grid ytics 
+
+set term png size 1200,600
+
+plot "./acq_5ms_env_50ms_enr_02.csv" using 1:2 w l title "Vitesse X"
+

gyro.c

@@ -7,10 +7,8 @@
 #include "Temps.h"
 #include "gyro.h"
 
-const uint PIN_CS = 1;
-
 struct t_angle_gyro{
-    int32_t rot_x, rot_y, rot_z;
+    int32_t rot_x, rot_y, rot_z, temp;
 };
 
 /// @brief structure d'échange des angles du gyrocope
@@ -26,13 +24,15 @@ uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
 
 
 
-struct t_angle_gyro_double angle_gyro;
+struct t_angle_gyro_double angle_gyro, vitesse_gyro;
 
 
 struct t_angle_gyro_double gyro_get_angle(void){
     return angle_gyro;
 }
-
+struct t_angle_gyro_double gyro_get_vitesse(void){
+    return vitesse_gyro;
+}
 
 void Gyro_Init(void){
     // 
@@ -45,9 +45,14 @@ void Gyro_Init(void){
     gpio_set_dir(PIN_CS, GPIO_OUT);
     cs_deselect();
 
+    vitesse_calibration.rot_x = 0;
+    vitesse_calibration.rot_y = 0;
+    vitesse_calibration.rot_z = 0;
+    vitesse_calibration.temp = 0;
+
     //spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz
     uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
-    printf("vitesse SPI : %d ", speed);
+    printf("vitesse SPI : %d\n", speed);
 
     //Ça doit être les valeurs par défaut, mais ça marche !
     spi_set_format(spi0, 8, SPI_CPHA_1, SPI_CPOL_1, SPI_MSB_FIRST);
@@ -57,9 +62,12 @@ void Gyro_Init(void){
         puts("Gyroscope non trouve");
     }else{
         puts("Gyroscope trouve");
-        gyro_config();
+        gyro_config();        
     }
-    gyro_calibration();
+    sleep_ms(150); // Temps d'init du gyroscope
+    /*while(1){
+        gyro_calibration();
+    }*/
 }
 
 int gyro_init_check(){
@@ -86,22 +94,27 @@ void gyro_config(){
     uint16_t tampon[2] = {0x20, config};
     uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
     int statu, nb_read;
-    while(spi_nb_busy(spi0) == SPI_BUSY);
+
+    //while(spi_nb_busy(spi0) == SPI_BUSY);
     cs_select();
     int rep = spi_nb_write_data(spi0, tampon, 2);
     if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
         printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
         //return statu;
     }
-    while(spi_nb_busy(spi0) == SPI_BUSY);
+    while(spi_nb_busy(spi0));
     cs_deselect();
 
-    spi_read_register(spi0, 0x20, tampon2, 1);
+    int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
+    
+
+
+    printf("Nb lu: %d\n", nb_lu);
     
     if(tampon2[1] == config){
         puts("gyro_config ok !");
     }else{
-        puts("gyro_config FAILED !");
+        printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
     }
     // Registre 
 
@@ -138,9 +151,13 @@ void Gyro_Read(uint16_t step_ms){
     //gyro_get_angles(&vitesse_angulaire, NULL);
 
     // Angle en degré
-    angle_gyro.rot_x = angle_gyro.rot_x + (double)vitesse_angulaire.rot_x * step_ms * 0.001 * 0.00875 * 0.125;
-    angle_gyro.rot_y = angle_gyro.rot_y + (double)vitesse_angulaire.rot_y * step_ms * 0.001 * 0.00875 * 0.125;
-    angle_gyro.rot_z = angle_gyro.rot_z + (double)vitesse_angulaire.rot_z * step_ms * 0.001 * 0.00875 * 0.125;
+    vitesse_gyro.rot_x = (double)vitesse_angulaire.rot_x * 0.00875 / 32.0;
+    vitesse_gyro.rot_y = (double)vitesse_angulaire.rot_y * 0.00875 / 32.0;
+    vitesse_gyro.rot_z = (double)vitesse_angulaire.rot_z * 0.00875 / 32.0;
+
+    angle_gyro.rot_x = angle_gyro.rot_x + vitesse_gyro.rot_x * step_ms * 0.001;
+    angle_gyro.rot_y = angle_gyro.rot_y + vitesse_gyro.rot_y * step_ms * 0.001;
+    angle_gyro.rot_z = angle_gyro.rot_z + vitesse_gyro.rot_z * step_ms * 0.001;
 
     //printf("%d, %#4x, %#4x, %#4x\n", step_ms, vitesse_angulaire.rot_x, vitesse_angulaire.rot_y, vitesse_angulaire.rot_z);
     
@@ -151,6 +168,15 @@ void Gyro_Read(uint16_t step_ms){
     //printf("tampon : %s\n", tampon);
 }
 
+int16_t gyro_get_temp(void){
+    int8_t tampon[3]="\0\0";
+    int16_t temperature;
+    spi_read_register(spi0, 0x26, tampon, 6);
+    temperature = -tampon[1];
+    printf("temperature %d\n",temperature);
+
+}
+
 void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){
     uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0";
     int16_t rot_x, rot_y, rot_z;
@@ -161,32 +187,31 @@ void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle
     rot_z = -(tampon[5] + (tampon[6] << 8));
 
     if(angle_gyro_moy == NULL){
-        angle_gyro->rot_x = (int32_t) rot_x * 8;
-        angle_gyro->rot_y = (int32_t) rot_y * 8;
-        angle_gyro->rot_z = (int32_t) rot_z * 8;
+        angle_gyro->rot_x = (int32_t) rot_x * 32;
+        angle_gyro->rot_y = (int32_t) rot_y * 32;
+        angle_gyro->rot_z = (int32_t) rot_z * 32;
     }else{
-        angle_gyro->rot_x = (int32_t) rot_x * 8 - angle_gyro_moy->rot_x;
-        angle_gyro->rot_y = (int32_t) rot_y * 8 - angle_gyro_moy->rot_y;
-        angle_gyro->rot_z = (int32_t) rot_z * 8 - angle_gyro_moy->rot_z;
+        angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
+        angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
+        angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z;
     }
-
-    
-
-    
-
 }
 
+
+
 void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
     if(titre != NULL){
         printf("%s ",titre);
     }
-    printf("rx : %f, ry : %f, rz: %f\n", angle_gyro.rot_x, 
-    angle_gyro.rot_y, angle_gyro.rot_z);
+    printf("angle, %f, %f, %f\n", angle_gyro.rot_x, angle_gyro.rot_y, angle_gyro.rot_z);
 }
 
 void gyro_calibration(void){
-    uint32_t nb_ech = 100;
+    uint32_t t_calibration_ms = 40000;
+    uint32_t nb_ech = t_calibration_ms/5;
     uint32_t m_temps_ms = Temps_get_temps_ms();
+    uint32_t temps_500ms = m_temps_ms;
+    int16_t temperature;
 
     printf("Calibration...\n");
 
@@ -194,7 +219,7 @@ void gyro_calibration(void){
     vitesse_calibration.rot_y = 0;
     vitesse_calibration.rot_z = 0;
     
-    // Acquisition des échantillons, 1 par milliseconde
+    // Acquisition des échantillons, 1 par milliseconde (1 ms, c'est trop court on dirait !)
     for(uint32_t i=0; i<nb_ech; i++){
         while(m_temps_ms == Temps_get_temps_ms());
         m_temps_ms = Temps_get_temps_ms();
@@ -202,13 +227,19 @@ void gyro_calibration(void){
         vitesse_calibration.rot_x += vitesse_angulaire.rot_x;
         vitesse_calibration.rot_y += vitesse_angulaire.rot_y;
         vitesse_calibration.rot_z += vitesse_angulaire.rot_z;
-
+        if(m_temps_ms > temps_500ms){
+            printf(".");
+            gyro_get_temp();
+            temps_500ms += 500;
+        }
+        sleep_ms(5);
     }
-    vitesse_calibration.rot_x = vitesse_calibration.rot_x / nb_ech;
-    vitesse_calibration.rot_y = vitesse_calibration.rot_y / nb_ech;
-    vitesse_calibration.rot_z = vitesse_calibration.rot_z / nb_ech;
+    vitesse_calibration.rot_x = vitesse_calibration.rot_x / (int32_t)nb_ech;
+    vitesse_calibration.rot_y = vitesse_calibration.rot_y / (int32_t)nb_ech;
+    vitesse_calibration.rot_z = vitesse_calibration.rot_z / (int32_t)nb_ech;
+    temperature = gyro_get_temp();
 
-    printf("fin calibration\n");
+    printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration.rot_x, vitesse_calibration.rot_y ,vitesse_calibration.rot_z, temperature);
 
 }
 

gyro.h

@@ -5,4 +5,6 @@ struct t_angle_gyro_double{
 void Gyro_Init(void);
 void Gyro_Read(u_int16_t);
 void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre);
-struct t_angle_gyro_double gyro_get_angle(void);
+struct t_angle_gyro_double gyro_get_angle(void);
+struct t_angle_gyro_double gyro_get_vitesse(void);
+int16_t gyro_get_temp(void);

spi_nb.c

@@ -14,7 +14,7 @@ uint16_t spi0_slave_register;
 uint8_t* spi0_buffer;
 uint8_t  spi0_nb_data_to_read;
 
-#define PIN_CS 1
+
 
 void cs_select(void) {
     asm volatile("nop \n nop \n nop");
@@ -158,7 +158,7 @@ uint8_t spi_nb_read_data_8bits(spi_inst_t * spi, uint8_t * buffer){
 /// @param size size of the data to transmit
 /// @return SPI_OK or SPI_ERR_TRANSMIT_FIFO_FULL
 inline int spi_nb_write_data(spi_inst_t * spi, uint16_t * buffer, uint8_t size){
-    int status_spi = SPI_OK;
+    int status_spi;
     uint8_t index=0;
     do
     {
@@ -168,7 +168,7 @@ inline int spi_nb_write_data(spi_inst_t * spi, uint16_t * buffer, uint8_t size){
         }else{
             status_spi = SPI_ERR_TRANSMIT_FIFO_FULL;
         }
-        while (spi_nb_busy(spi));
+        //while (spi_nb_busy(spi));// <== ça, c'est bizarre !
         //statu_spi = spi_nb_write_byte(spi, buffer[index]);
         //printf("envoi : %x\n", buffer[index]);
         //sleep_ms(1);
@@ -209,5 +209,65 @@ int spi_read_register(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *bu
     if(nb_read != nb_to_read+1){
         printf("Erreur: spi_read_register, nb de valeurs lues incoherentes");
     }
+    return nb_read;
 
+}
+
+void spi_test(){
+    // Gyro_Init
+
+    uint16_t tampon_ecriture[] = {'a','b','c','d','e','f','g','h','i','j',0};
+    uint8_t tampon_lecture[10] = {0,0,0,0,0,0,0,0,0,0};
+    uint8_t nb_lu, statu;
+
+    uint8_t config = 0b11101111;
+    uint16_t config_gyro[2] = {0x20, config};
+
+
+    gpio_set_function(16, GPIO_FUNC_SPI); // SDI
+    gpio_set_function(18, GPIO_FUNC_SPI); // SCK
+    gpio_set_function(19, GPIO_FUNC_SPI); // SDO
+    gpio_set_function(PIN_CS, GPIO_OUT); // CSn
+
+    gpio_init(PIN_CS);
+    gpio_set_dir(PIN_CS, GPIO_OUT);
+    cs_deselect();
+
+    //spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz
+    uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
+    printf("vitesse SPI : %d\n", speed);
+
+    //Ça doit être les valeurs par défaut, mais ça marche !
+    spi_set_format(spi0, 8, SPI_CPHA_1, SPI_CPOL_1, SPI_MSB_FIRST);
+
+    // Gyro_Init
+
+    // gyro_config
+
+
+    cs_select();
+    statu = spi_nb_write_data(spi0, config_gyro, 2);
+    if(statu == SPI_ERR_TRANSMIT_FIFO_FULL){
+        printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
+    }
+
+    while(spi_nb_busy(spi0));
+    cs_deselect();
+
+    nb_lu = spi_read_register(spi0, 0x20, tampon_lecture, 1);
+
+
+    printf("Nb lu: %d\n", nb_lu);
+    //puts(tampon_lecture);
+
+    if(tampon_lecture[1] == config){
+        puts("gyro_config ok !");
+    }else{
+        puts("gyro_config FAILED !");
+        printf("gyro_config FAILED ! :%#4x\n", tampon_lecture[1]);
+    }
+    
+
+
+    
 }

spi_nb.h

@@ -7,9 +7,11 @@
 #define SPI_IN_PROGRESS 1
 #define SPI_FAILED 2
 
-#define SPI_ERR_TRANSMIT_FIFO_FULL 1
+#define SPI_ERR_TRANSMIT_FIFO_FULL -1
 #define SPI_OK 0
 
+#define PIN_CS 1
+
 int spi_nb_busy(spi_inst_t * spi);
 void spi_nb_flush_recieve_fifo(spi_inst_t * spi);
 int spi_nb_write_byte(spi_inst_t * spi, uint16_t data);
@@ -19,4 +21,5 @@ int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, ui
 int spi_read_register(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_to_read);
 
 void cs_select(void);
-void cs_deselect(void);
+void cs_deselect(void);
+void spi_test();

test.c

@@ -5,14 +5,20 @@
 
 #include "gyro.h"
 #include "Temps.h"
+#include "spi_nb.h"
+#include "Servomoteur.h"
 
 const uint LED_PIN = 25;
 
+#define V_INIT -999.0
+
 int main() {
     bi_decl(bi_program_description("This is a test binary."));
     bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
+    double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
+    struct t_angle_gyro_double angle_gyro;
 
-    uint32_t temps_ms = 0;
+    uint32_t temps_ms = 0, temps_ms_old;
 
     stdio_init_all();
 
@@ -21,24 +27,54 @@ int main() {
     gpio_put(LED_PIN, 1);
 
     sleep_ms(3000);
+    Servomoteur_Init();
+    //puts("Debut");
+    //spi_test();
 
-
+    //while(1);
     Temps_init();
     Gyro_Init();
     temps_ms = Temps_get_temps_ms();
+    temps_ms_old = temps_ms;
     while (1) { 
         u_int16_t step_ms = 5;
+        float coef_filtre = 0.001;
+        
+        while(temps_ms_old == Temps_get_temps_ms());
+        temps_ms_old = Temps_get_temps_ms();
         
         // Tous les pas de step_ms
-        if(temps_ms == Temps_get_temps_ms()){
-            temps_ms += step_ms;
+        if(Temps_get_temps_ms() % step_ms){
             Gyro_Read(step_ms);
+            //gyro_affiche(gyro_get_vitesse(), "Angle :");
+            // Filtre 
+            angle_gyro = gyro_get_vitesse();
+            if(vitesse_filtre_x == V_INIT){
+                vitesse_filtre_x = angle_gyro.rot_x;
+                vitesse_filtre_y = angle_gyro.rot_y;
+                vitesse_filtre_z = angle_gyro.rot_z;
+            }else{
+                vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre;
+                vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
+                vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
+            }
+
+            printf("%f, %f, %f, %f\n", (double)temps_ms_old / 1000, vitesse_filtre_x, vitesse_filtre_y, vitesse_filtre_z);
+            //gyro_affiche(angle_gyro, "Vitesse (°/s),");
+        }
+
+        // Toutes les 50 ms
+        if((Temps_get_temps_ms() % 50) == 0){
             
         }
 
         // Toutes les 500 ms
         if((Temps_get_temps_ms() % 500) == 0){
-            gyro_affiche(gyro_get_angle(), "Angle :");
+            //gyro_affiche(gyro_get_angle(), "Angle :");
+        }
+        // Toutes les secondes
+        if((Temps_get_temps_ms() % 500) == 0){
+            //gyro_get_temp();
         }
     }
 }