Ajout du pilotage des servomoteurs

main.c

@@ -26,16 +26,16 @@ void main(void)
 	
 	gpio_init(PIN_LED_VERTE);
 	gpio_set_function(PIN_LED_VERTE, GPIO_FUNC_PWM);
-	pwm_set_clkdiv(4, 10000);
+	pwm_set_clkdiv(4, 100);
 	pwm_set_wrap(4, 100);
 	pwm_set_enabled(4, true);
 	pwm_set_chan_level(4, PWM_CHAN_B, 100);
 
-	uint vitesse_angle = 0;
-	uint vitesse_m1 = 0;
-	uint vitesse_m2 = 0;
-	uint vitesse_m3 = 0;
-	uint vitesse_m4 = 0;
+	int vitesse_angle = 0;
+	int vitesse_m1 = 0;
+	int vitesse_m2 = 0;
+	int vitesse_m3 = 0;
+	int vitesse_m4 = 0;
 
 	while(1){
 
@@ -78,5 +78,18 @@ void main(void)
 		Motor2_speed(vitesse_m2);
 		Motor3_speed(vitesse_m3);
 		Motor4_speed(vitesse_m4);
+
+		switch(reception[3]){
+			case 0: Servo_pince_ouverte(); break;
+			case 1: Servo_pince_plante(); break;
+			case 2: Servo_pince_pot(); break;
+		}
+
+		switch(reception[4]){
+			case 0: Servo_ascenseur_bas(); break;
+			case 1: Servo_ascenseur_haut(); break;
+			case 2: Servo_ascenseur_lache_pot_jardiniere(); break;
+			case 3: Servo_ascenseur_lache_plante(); break;
+		}
 	}
 }

moteur.c

@@ -1,6 +1,6 @@
 #include "moteur.h"
 
-// Define pins
+// Define pins servo
 #define MOTEUR1_PIN_SENS1 4
 #define MOTEUR1_PIN_SENS2 5
 #define MOTEUR1_PIN_ACTIVATION 0
@@ -11,12 +11,18 @@
 #define MOTEUR3_PIN_SENS2 10
 #define MOTEUR3_PIN_ACTIVATION 2
 #define MOTEUR4_PIN_SENS1 8
-#define MOTEUR4_PIN_SENS2 11
+#define MOTEUR4_PIN_SENS2 9
 #define MOTEUR4_PIN_ACTIVATION 3
 
+// Define pins servo
+#define SERVO1_PIN 20
+#define SERVO2_PIN 21
+
 // Init all motion motors pins
 void Init_motion_motor(void)
 {
+	// Init motion motors
+
 	// Init 1/0 pin for control motion motors
 	gpio_init(MOTEUR1_PIN_SENS1);
 	gpio_init(MOTEUR1_PIN_SENS2);
@@ -69,8 +75,68 @@ void Init_motion_motor(void)
 	pwm_set_chan_level(1, PWM_CHAN_A, 0);
 	pwm_set_chan_level(1, PWM_CHAN_B, 0);
 
+	// Servo Init
+
+	// Init pin for the servos
+	gpio_init(SERVO1_PIN);
+	gpio_init(SERVO2_PIN);
+	gpio_set_function(SERVO1_PIN, GPIO_FUNC_PWM);
+	gpio_set_function(SERVO2_PIN, GPIO_FUNC_PWM);
+
+	// Set wrap of pwm slice
+	pwm_set_wrap(2, 25000);
+
+	// Set clock div for this pwm slice
+	pwm_set_clkdiv(2, 100);
+
+	// Enable pwm slice
+	pwm_set_enabled(2, true);
+
+	// Set channel level
+	pwm_set_chan_level(2, PWM_CHAN_A, 1950); // Ascenseur
+	pwm_set_chan_level(2, PWM_CHAN_B, 1000); // Pince
+	// Edit 17/01/2024
+	// Réglage servomoteurs en 25000 * 100
+	// Pince
+	// Pot : 1700
+	// Plante : 1970
+	// Ouvert : 1000
+	// Ascenseur
+	// Haut : 2550
+	// Bas : 1550
+	// Lâche plante : 2050
+	// Lâche pot jardinière : 1950
+
+
+
+
+
 }
 
+void Servo_ascenseur_haut(){
+	pwm_set_chan_level(2, PWM_CHAN_A, 2550);
+}
+void Servo_ascenseur_bas(){
+	pwm_set_chan_level(2, PWM_CHAN_A, 1550);
+}
+void Servo_ascenseur_lache_plante(){
+	pwm_set_chan_level(2, PWM_CHAN_A, 2050);
+}
+void Servo_ascenseur_lache_pot_jardiniere(){
+	pwm_set_chan_level(2, PWM_CHAN_A, 1950);
+}
+
+void Servo_pince_ouverte(){
+	pwm_set_chan_level(2, PWM_CHAN_B, 1000);
+}
+void Servo_pince_pot(){
+	pwm_set_chan_level(2, PWM_CHAN_B, 1700);
+}
+void Servo_pince_plante(){
+	pwm_set_chan_level(2, PWM_CHAN_B, 1950);
+}
+
+
 // Set motor 1 speed forward
 void Motor1_forward(int speed)
 {
@@ -189,4 +255,6 @@ void Motor4_speed(int speed)
 	{
 		Motor4_forward(speed);
 	}
-}
+}
+
+// Set

moteur.h

@@ -13,4 +13,12 @@ void Motor3_backward(int speed);
 void Motor3_speed(int speed);
 void Motor2_forward(int speed);
 void Motor3_backward(int speed);
-void Motor4_speed(int speed);
+void Motor4_speed(int speed);
+
+void Servo_ascenseur_haut();
+void Servo_ascenseur_bas();
+void Servo_ascenseur_lache_plante();
+void Servo_ascenseur_lache_pot_jardiniere();
+void Servo_pince_ouverte();
+void Servo_pince_pot();
+void Servo_pince_plante();