From b0e750106d7c4084b9bce7ac1499fcf81d405222 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 22 Jun 2024 19:04:37 +0200
Subject: [PATCH] =?UTF-8?q?Ajout=20du=20contr=C3=B4le=20anti-panique=20?=
=?UTF-8?q?=C3=A0=20la=20fin=20du=20trajet?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Asser_Position.c | 17 ++++++++++++++++-
Asser_Position.h | 2 ++
main.c | 6 +++++-
3 files changed, 23 insertions(+), 2 deletions(-)
diff --git a/Asser_Position.c b/Asser_Position.c
index 8289949..74870ff 100644
--- a/Asser_Position.c
+++ b/Asser_Position.c
@@ -5,6 +5,8 @@
#define GAIN_P_POSITION 15
#define GAIN_P_ORIENTATION 10
+#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
+
struct position_t position_maintien;
/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
@@ -60,4 +62,17 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
void Asser_Position_maintien(){
Asser_Position(position_maintien);
-}
\ No newline at end of file
+}
+
+float Asser_Position_get_erreur_angle(){
+ return delta_orientation_radian;
+}
+
+/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
+/// @return 1 si panic, 0 si nominal
+int Asser_Position_panic_angle(){
+ if(delta_orientation_radian > MAX_ERREUR_ANGLE){
+ return 1;
+ }
+ return 0;
+}
diff --git a/Asser_Position.h b/Asser_Position.h
index d728db3..aa2e802 100644
--- a/Asser_Position.h
+++ b/Asser_Position.h
@@ -2,3 +2,5 @@
void Asser_Position(struct position_t position_consigne);
void Asser_Position_set_Pos_Maintien(struct position_t position);
void Asser_Position_maintien();
+
+int Asser_Position_panic_angle();
diff --git a/main.c b/main.c
index 64f86e7..cedc1c1 100644
--- a/main.c
+++ b/main.c
@@ -56,6 +56,7 @@ void main(void)
{
int ledpower = 500;
VL53L8CX_ResultsData Results;
+ bool fin_match = false;
@@ -108,7 +109,7 @@ void main(void)
while(1){
// Fin du match
- if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
+ if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
Moteur_Stop();
while(1);
}
@@ -122,6 +123,9 @@ void main(void)
etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{
Asser_Position_maintien();
+ if(Asser_Position_panic_angle()){
+ fin_match=1;
+ }
}
AsserMoteur_Gestion(step_ms);
}