From f26e6a0c5da3f39c77d5e4f8e9508dd11c20014f Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 11 Mar 2023 22:03:51 +0100
Subject: [PATCH] =?UTF-8?q?Test=20rotation=20par=20rapport=20=C3=A0=20un?=
 =?UTF-8?q?=20point=20:=20OK?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Test.c | 79 +++++++++++++++++++++++++++++++++++++++-------------------
 1 file changed, 54 insertions(+), 25 deletions(-)

diff --git a/Test.c b/Test.c
index 9615bf8..2a82916 100644
--- a/Test.c
+++ b/Test.c
@@ -36,8 +36,9 @@ int test_vitesse_moteur(enum t_moteur moteur);
 int test_asser_moteur(void);
 int test_localisation(void);
 int test_avance(void);
+int test_cde_vitesse(void);
 int test_cde_vitesse_rotation(void);
-int test_cde_rotation_ref_robot(void);
+int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm);
 int test_cde_vitesse_rectangle(void);
 int test_cde_vitesse_cercle(void);
 int test_asser_position_avance(void);
@@ -61,16 +62,13 @@ int mode_test(){
     printf("B - pour avance (asser_moteur)\n");
     printf("C - pour les codeurs\n");
     printf("D - pour les codeurs (somme en mm)\n");
-    printf("E - Commande en vitesse - rotation pure\n");
-    printf("F - Commande en vitesse - carré\n");
-    printf("G - Commande en vitesse - cercle\n");
+    printf("E - Commande en vitesse...\n");
     printf("H - Asser Position - avance\n");
     printf("I - Asser Position - avance et tourne (gyro)\n");
     printf("J - Asser Position - avance et tourne (sans gyro)\n");
     printf("K - Trajets aller retour avec Gyro\n");
     printf("L - pour la localisation\n");
     printf("M - pour les moteurs\n");
-    printf("N - Rotation dans le référentiel du robot\n");
     printf("T - Trajectoire\n");
     printf("U - Scan du bus i2c\n");
     printf("V - APDS_9960\n");
@@ -103,17 +101,7 @@ int mode_test(){
 
     case 'E':
     case 'e':
-        while(test_cde_vitesse_rotation());
-        break;
-
-    case 'F':
-    case 'f':
-        while(test_cde_vitesse_rectangle());
-        break;
-
-    case 'G':
-    case 'g':
-        while(test_cde_vitesse_cercle());
+        while(test_cde_vitesse());
         break;
 
     case 'H':
@@ -146,11 +134,6 @@ int mode_test(){
         while(test_moteurs());
         break;
 
-    case 'N':
-    case 'n':
-        while(test_cde_rotation_ref_robot());
-        break;
-
     case 'T':
     case 't':
         while(test_trajectoire());
@@ -794,6 +777,48 @@ int test_asser_position_avance(){
     return 0;
 }
 
+int test_cde_vitesse(){
+    printf("A - Commande en vitesse - rectangle\n");
+    printf("B - Commande en vitesse - cercle\n");
+    printf("C - Commande en vitesse - rotation pure\n");
+    printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n");
+    printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n");
+
+    int lettre;
+    do{ 
+        lettre = getchar_timeout_us(0);
+        stdio_flush();
+
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
+
+    switch(lettre){
+        case 'a':
+        case 'A':
+            while(test_cde_vitesse_rectangle());
+            break;
+        
+        case 'b':
+        case 'B':
+            while(test_cde_vitesse_cercle());
+            break;
+        
+        case 'c':
+        case 'C':
+            while(test_cde_vitesse_rotation());
+            break;
+        
+        case 'd':
+        case 'D':
+            while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0));
+            break;
+
+        case 'e':
+        case 'E':
+            while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2));
+            break;
+    }
+}
+
 int test_cde_vitesse_rotation(){
     int lettre, _step_ms = 1;
     double vitesse =90.0/2 * 3.14159 /180.0;
@@ -806,23 +831,25 @@ int test_cde_vitesse_rotation(){
         sleep_ms(_step_ms);
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
 
     return 0;
 }
 
 
-int test_cde_rotation_ref_robot(){
+int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm){
     int lettre, _step_ms = 1;
-    double vitesse =90.0/2 * 3.14159 /180.0;
-    printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
+    double vitesse =90.0/4 * 3.14159 /180.0;
+    printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);
 
-    commande_rotation(vitesse, RAYON_ROBOT, 0);
+    commande_rotation(vitesse, centre_x_mm, centre_y_mm);
     do{
         QEI_update();
         AsserMoteur_Gestion(_step_ms);
         sleep_ms(_step_ms);
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
 
     return 0;
 }
@@ -851,6 +878,7 @@ int test_cde_vitesse_rectangle(){
         temps_ms += _step_ms;
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
 
     return 0;
 }
@@ -869,6 +897,7 @@ int test_cde_vitesse_cercle(){
 
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_Stop();
 
     return 0;
 }