Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

On attrape une plante

Browse Source
This commit is contained in:
Samuel 2024-04-13 21:50:50 +02:00
parent 5318b1eb3f
commit 00f923f7f1
6 changed files with 170 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -32,6 +32,7 @@ Score.c
Servomoteur.c
Strategie.c
Strategie_deplacement.c
Strategie_2024_plante.c
Strategie_2024_pots.c
Temps.c
Test.c

2
Geometrie_robot.h
View File

@ -1,3 +1,5 @@
#include "Geometrie.h"
#define DISTANCE_ROUES_CENTRE_MM 117.2
// Distance entre le centre du robot et un angle du robot

66
Strategie_2024_plante.c Normal file
View File

@ -0,0 +1,66 @@
#include "Commande_vitesse.h"
#include "Strategie_2024_plante.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "math.h"
#include <stdio.h>
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
enum etat_action_t Strat_2024_aller_a_plante(){
static enum {
SAAP_INIT_DETECTION,
SAAP_ASSERV,
SAAP_ATTRAPE,
SAAP_ATTRAPE_TEMPO,
} etat_aller_a_plante = SAAP_INIT_DETECTION;
enum validite_vl53l8_t validite;
float angle, distance, commande_vitesse_plante;
static int tempo;
switch(etat_aller_a_plante){
case SAAP_INIT_DETECTION:
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
etat_aller_a_plante = SAAP_ASSERV;
break;
case SAAP_ASSERV:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_PLANTE){
commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
if(commande_vitesse_plante > 300){
commande_vitesse_plante = 300;
}
if(commande_vitesse_plante <= 0){
commande_vitesse_stop();
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
etat_aller_a_plante = SAAP_ATTRAPE;
break;
}
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
}else{
}
break;
case SAAP_ATTRAPE:
i2c_annexe_attrape_plante(PLANTE_BRAS_1);
etat_aller_a_plante = SAAP_ATTRAPE_TEMPO;
tempo = 2500;
break;
case SAAP_ATTRAPE_TEMPO:
tempo--;
if(tempo <= 0){
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}

3
Strategie_2024_plante.h Normal file
View File

@ -0,0 +1,3 @@
#include "Strategie.h"
enum etat_action_t Strat_2024_aller_a_plante();

88
Test_strategie_2024.c
View File

@ -11,6 +11,7 @@
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "Robot_config.h"
#include "Strategie_2024_plante.h"
#include "Strategie_2024_pots.h"
#include "Strategie.h"
#include "Trajectoire.h"
@ -21,13 +22,15 @@
int test_calcul_position_pot(void);
int test_calage_debut(void);
int test_attrape_pot();
int test_attrape_pot(void);
int test_attrape_plante(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
printf("A - Position groupes pot.\n");
printf("B - Calage debut.\n");
printf("C - Attrape pot.\n");
printf("D - Attrape plante.\n");
int lettre;
do{
@ -49,6 +52,11 @@ int test_strategie_2024(){
while(test_attrape_pot());
break;
case 'd':
case 'D':
while(test_attrape_plante());
break;
case 'q':
case 'Q':
return 0;
@ -154,13 +162,67 @@ int test_calage_debut(){
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
Moteur_Stop();
if(lettre == 'q' && lettre == 'Q'){
return 0;
return 0;
}
int test_attrape_plante(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
printf("test_attrape_plante\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strat_2024_aller_a_plante();
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
@ -248,6 +310,8 @@ int test_attrape_pot(){
void affichage_test_strategie_2024(){
uint32_t temps;
enum validite_vl53l8_t validite_vl53l8;
float angle, distance;
while(true){
temps = time_us_32()/1000;
@ -265,6 +329,20 @@ void affichage_test_strategie_2024(){
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
i2c_annexe_get_VL53L8(&validite_vl53l8, &angle, &distance);
printf(">v:%d\n", validite_vl53l8);
switch(validite_vl53l8){
case VL53L8_BORDURE:
printf(">b_angle:%.2f\n>b_distance:%.2f\n", angle, distance);
break;
case VL53L8_PLANTE:
printf(">p_angle:%.2f\n>p_distance:%.2f\n", angle, distance);
break;
case VL53L8_DISTANCE_LOIN:
printf("\n>v_distance:%.2f\n", distance);
break;
}
sleep_ms(100);
}

15
i2c_annexe.c
View File

@ -44,6 +44,7 @@ void i2c_annexe_gestion(){
EMISSION_TEMPO_RPi,
RECEPTION_DONNEES_RPi,
EMISSION_PIC18F,
EMISSION_TEMPO_PIC18F,
} etat_i2c_annexe=EMISSION_PIC18F;
enum i2c_resultat_t retour_i2c;
@ -58,6 +59,8 @@ void i2c_annexe_gestion(){
etat_i2c_annexe = EMISSION_TEMPO_RPi;
donnees_a_envoyer=0;
temps = time_us_32();
}else if(retour_i2c == I2C_ECHEC){
printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE);
}
}else{
etat_i2c_annexe = EMISSION_TEMPO_RPi;
@ -76,6 +79,8 @@ void i2c_annexe_gestion(){
if(retour_i2c == I2C_SUCCES){
etat_i2c_annexe = EMISSION_PIC18F;
temps = time_us_32();
}else if(retour_i2c == I2C_ECHEC){
printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE);
}
break;
@ -88,8 +93,16 @@ void i2c_annexe_gestion(){
temps = time_us_32();
donnees_a_envoyer_pic=0;
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0;
}else if(retour_i2c == I2C_ECHEC){
printf("ECHEC I2C address: %d\n", ADRESSE_PIC18F4550);
}
}else{
etat_i2c_annexe = EMISSION_TEMPO_PIC18F;
}
break;
case EMISSION_TEMPO_PIC18F:
if(temps + tempo < time_us_32() ){
etat_i2c_annexe = EMISSION_DONNEES_RPi;
}
break;
@ -172,6 +185,8 @@ void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doi
donnees_a_envoyer_pic=1;
}
/// @brief Envoi l'ordre d'attraper une plance
/// @param action : PLANTE_BRAS_1 ou PLANTE_BRAS_6
void i2c_annexe_attrape_plante(uint8_t action){
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = action;
donnees_a_envoyer_pic=1;