Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WIP

Browse Source
This commit is contained in:
Samuel 2023-05-17 19:01:21 +02:00
parent 5e0c7238be
commit 49778b85e6
5 changed files with 24 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Holonome2023.c
View File

@ -30,7 +30,7 @@
#include "Trajet.h" #include "Trajet.h"
const uint LED_PIN = 25; const uint LED_PIN = 25;
const uint LED_PIN_ROUGE = 28; #define LED_PIN_ROUGE 28
const uint LED_PIN_NE_PAS_UTILISER = 22; const uint LED_PIN_NE_PAS_UTILISER = 22;
uint temps_cycle; uint temps_cycle;

7
Monitoring.c
View File

@ -5,6 +5,8 @@
#include "Asser_Moteurs.h" #include "Asser_Moteurs.h"
#include "i2c_annexe.h" #include "i2c_annexe.h"
#define LED_PIN_ROUGE 28
uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0; uint32_t temps_cycle_max=0;
int lock=0; int lock=0;
@ -76,4 +78,9 @@ void set_debug_var(uint32_t variable){
void set_debug_varf(float variable){ void set_debug_varf(float variable){
debug_varf = variable; debug_varf = variable;
}
void Monitoring_Error(char * msg){
gpio_put(LED_PIN_ROUGE, 1);
} }

3
Monitoring.h
View File

@ -7,4 +7,5 @@ uint32_t temps_cycle_get_min();
uint32_t temps_cycle_get_max(); uint32_t temps_cycle_get_max();
void set_debug_var(uint32_t variable); void set_debug_var(uint32_t variable);
void set_debug_varf(float variable); void set_debug_varf(float variable);
void Monitoring_display(); void Monitoring_display();
void Monitoring_Error(char * msg);

16
Strategie.c
View File

@ -86,8 +86,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; struct objectif_t objectif_3 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -96,8 +96,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}else{ }else{
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; struct objectif_t objectif_3 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_DROITE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -211,11 +211,11 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Trajet_config(300, 250); Trajet_config(300, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_DROITE; etat_strategie = ATTRAPER_CERISE_DROITE;
} }
}else{ }else{
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_DROITE; etat_strategie = ATTRAPER_CERISE_DROITE;
} }
} }
@ -295,14 +295,14 @@ enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t coul
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
740, 3000 - 550, 740, 3000 - 550,
510, 3000 - 1580, 510, 3000 - 1580,
180, 1500, 180, 1400,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
}else{ }else{
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 740, 3000 - 550, 2000 - 740, 3000 - 550,
2000 - 510, 3000 - 1580, 2000 - 510, 3000 - 1580,
2000 - 180, 1500, 2000 - 180, 1400,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
} }

10
gyro_ADXRS453.c
View File

@ -1,4 +1,5 @@
#include "gyro_ADXRS453.h" #include "gyro_ADXRS453.h"
#include "Monitoring.h"
#include "spi_nb.h" #include "spi_nb.h"
#include <stdio.h> #include <stdio.h>
@ -29,13 +30,13 @@ int gyro_spi_wr_32bits(uint16_t *transmit_buffer, uint8_t *recieve_buffer){
cs_select(); cs_select();
if(spi_nb_write_data(spi0, (uint16_t*) transmit_buffer, 4) == SPI_ERR_TRANSMIT_FIFO_FULL){ if(spi_nb_write_data(spi0, (uint16_t*) transmit_buffer, 4) == SPI_ERR_TRANSMIT_FIFO_FULL){
puts("gyro_spi_wr_32bits: SPI_ERR_TRANSMIT_FIFO_FULL"); //puts("gyro_spi_wr_32bits: SPI_ERR_TRANSMIT_FIFO_FULL");
}else{ }else{
while(spi_nb_busy(spi0)); while(spi_nb_busy(spi0));
nb_recu = spi_nb_read_data_8bits(spi0, recieve_buffer); nb_recu = spi_nb_read_data_8bits(spi0, recieve_buffer);
} }
if(nb_recu != 4){ if(nb_recu != 4){
puts("gyro_spi_wr_32bits: nb_recu incohérent"); //puts("gyro_spi_wr_32bits: nb_recu incohérent");
} }
cs_deselect(); cs_deselect();
} }
@ -56,8 +57,9 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
gyro_spi_wr_32bits(tampon_envoi, tampon_reception); gyro_spi_wr_32bits(tampon_envoi, tampon_reception);
Gyro_traitementDonnees(tampon_reception); Gyro_traitementDonnees(tampon_reception);
if(Gyro_SensorData.SQ != 0x4){ if(Gyro_SensorData.SQ != 0x4){
printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ); //printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
affiche_tampon_32bits(tampon_reception); //affiche_tampon_32bits(tampon_reception);
Monitoring_Error("Gyro Failed - SQ bits != 0x4\n");
return 1; return 1;
} }
if(Gyro_SensorData.ST != 0x1){ if(Gyro_SensorData.ST != 0x1){