servomoteur fonctionnel

.vscode/settings.json

@@ -12,6 +12,7 @@
         "asser_position.h": "c",
         "stdlib.h": "c",
         "strategie.h": "c",
-        "strategie_deplacement.h": "c"
+        "strategie_deplacement.h": "c",
+        "servomoteur.h": "c"
     }
 }

CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(Mon_Projet
   QEI.c
   Strategie_deplacement.c
   Strategie.c
+  Servomoteur.c
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c

Servomoteur.c

@@ -6,11 +6,8 @@
 
 void Servomoteur_Init(void){
     uint slice_num;
-    gpio_set_function(SERVO5, GPIO_FUNC_PWM);
+    gpio_set_function(SERVO0, GPIO_FUNC_PWM);
-    gpio_set_function(SERVO6, GPIO_FUNC_PWM);
+    
-    gpio_set_function(SERVO7, GPIO_FUNC_PWM);
-    gpio_set_function(SERVO_TURBINE, GPIO_FUNC_PWM);
-
     pwm_config pwm_servo = pwm_get_default_config();
     
     // On veut un rebouclage au bout de 20 ms (50 Hz).
@@ -21,22 +18,15 @@ void Servomoteur_Init(void){
     // À la valeur finale, rebouclage à 0 (et non un compte à rebours)
     pwm_config_set_phase_correct(&pwm_servo, false);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO5);
+    slice_num = pwm_gpio_to_slice_num(SERVO0);
     pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE);
+    slice_num = pwm_gpio_to_slice_num(SERVO1);
     pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO6);
+    Servomoteur_set(SERVO0, SERVO_VALEUR_1_5MS);
-    pwm_init(slice_num, &pwm_servo, true);
 
-    slice_num = pwm_gpio_to_slice_num(SERVO7);
+    Servomoteur_set(SERVO1, SERVO_VALEUR_1_5MS);
-    pwm_init(slice_num, &pwm_servo, true);
-
-    Servomoteur_set(SERVO5, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO6, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO7, SERVO_VALEUR_1_5MS);
-    Servomoteur_set(SERVO_TURBINE, SERVO_VALEUR_1MS);
 }
 
 void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
@@ -45,30 +35,3 @@ void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
     //        B ou 1 si la GPIO est impaire
     pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur);
 }
-
-void Moteur_Init(){
-    uint slice_num;
-    gpio_set_function(SERVO8_MOT_PROPULSEUR, GPIO_FUNC_PWM);
-
-    pwm_config pwm_servo = pwm_get_default_config();
-
-    // On veut un rebouclage au bout de 20 ms (50 Hz).
-    // Division de l'horloge système (133 MHz) par 25 : 5230 kHz
-    pwm_config_set_clkdiv_int(&pwm_servo, 25);
-    // Valeur max du PXM pour avoir 5 kHz :  5230 kHz / 5 kHz : 1064
-    pwm_config_set_wrap(&pwm_servo, 1064);
-    // À la valeur finale, rebouclage à 0 (et non un compte à rebours)
-    pwm_config_set_phase_correct(&pwm_servo, false);
-
-    slice_num = pwm_gpio_to_slice_num(SERVO8_MOT_PROPULSEUR);
-    pwm_init(slice_num, &pwm_servo, true);
-
-    Moteur_Set(0);
-
-}
-
-/// @brief Envoie la consigne PWM au moteur.
-/// @param consigne : consigne entre 0 et 1.
-void Moteur_Set(double consigne){
-    Servomoteur_set(SERVO8_MOT_PROPULSEUR, (uint16_t) (1064 * consigne));
-}

Servomoteur.h

@@ -9,17 +9,11 @@
 
 
 // Servomoteurs
-#define SERVO5 4
+#define SERVO0 0
-#define SERVO6 5
+#define SERVO1 1
-#define SERVO7 6
+#define DOIGT_AVANCE 0, SERVO_VALEUR_1_5MS
-#define SERVO8_MOT_PROPULSEUR 15
+#define DOIGT_RETRAIT 0, SERVO_VALEUR_0_5MS
-#define SERVO_TURBINE 7
-
-#define DOIGT_AVANCE 1, SERVO_VALEUR_1_5MS
-#define DOIGT_RETRAIT 1, SERVO_VALEUR_1_5MS
 
 
 void Servomoteur_Init(void);
 void Servomoteur_set(uint8_t servomoteur, uint16_t valeur);
-void Moteur_Init(void);
-void Moteur_Set(double consigne);

main.c

@@ -14,6 +14,7 @@
 #include "Localisation.h"
 #include "Moteurs.h"
 #include "Strategie.h"
+#include "Servomoteur.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -37,10 +38,10 @@ uint16_t tension_batterie_lire(void);
 
 void identifiant_init(void);
 uint get_identifiant(void);
-
 int get_tirette(void);
 int get_couleur(void);
 
+void gestion_PAMI(uint32_t);
 void gestion_VL53L8CX(void);
 
 const uint32_t step_ms=1;
@@ -49,7 +50,6 @@ float distance1_mm=0, distance2_mm=0;
 // DEBUG
 extern float abscisse; 
 extern struct point_xyo_t point;
-float vitesse;
 VL53L8CX_Configuration Dev;
 
 void main(void)
@@ -58,7 +58,6 @@ void main(void)
 	bool fin_match = false;
 
 
-
     stdio_init_all();
 	Temps_init();
 
@@ -67,6 +66,8 @@ void main(void)
 	Trajet_init(get_identifiant());
 	i2c_maitre_init();
 
+	Servomoteur_Init();
+
 	
 
 	uint32_t temps_ms = Temps_get_temps_ms();
@@ -81,6 +82,7 @@ void main(void)
 
 	gpio_init(TIRETTE_PIN);
 	gpio_set_dir(LED1PIN, GPIO_IN);
+	gpio_pull_up(TIRETTE_PIN);
 	
 
 	
@@ -88,51 +90,25 @@ void main(void)
 	// TODO: A remettre - quand on aura récupéré un capteur
 	//multicore_launch_core1(gestion_VL53L8CX);
 
-	sleep_ms(85000);
+
 	printf("Demarrage...\n");
 	
-	
-	float vitesse_init =300;
-	vitesse = vitesse_init;
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette());
-	gpio_put(LED1PIN, 0);
-
-	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
-	if(get_identifiant() == 3){
-		sleep_ms(90000);
-	}
-
-	temps_depart_ms = Temps_get_temps_ms();
-
 	while(1){
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){
-			Moteur_Stop();
-			while(1){
-
-			}
-		}
 		if(temps_ms != Temps_get_temps_ms()){
 			temps_ms = Temps_get_temps_ms();
 			if(temps_ms % step_ms == 0){
 				QEI_update();
 				Localisation_gestion();
 				
-				if(Strategie_super_star(step_ms, COULEUR_JAUNE) == ACTION_TERMINEE){
+				gestion_PAMI(step_ms);
-					Asser_Position_maintien();
-					if(Asser_Position_panic_angle()){
-						fin_match=1;
-					}
-				}
 				AsserMoteur_Gestion(step_ms);
 			}
 		}
-
-		
 		
 	}
 }
@@ -174,6 +150,9 @@ void gestion_PAMI(uint32_t step_ms){
 		break;
 
 	case PAMI_TRAJECTOIRE:
+		if(Temps_get_temps_ms() - temps_tirette > 15000){
+			etat_PAMI = PAMI_DANCE;
+		}
 		switch (get_identifiant())
 		{
 			case 0: Strategie_super_star(step_ms, get_couleur()); break;