Correction des positions de démarrage

Lecture des consignes de vitesse pour commander les moteurs

.vscode/settings.json

@@ -1,5 +1,9 @@
 {
     "files.associations": {
-
+        "stdlib.h": "c",
+        "stdio.h": "c",
+        "i2c_slave.h": "c",
+        "i2c.h": "c",
+        "moteur.h": "c"
     }
 }

CMakeLists.txt

@@ -15,6 +15,8 @@ add_executable(Mon_Projet
   i2c_maitre.c
   i2c_slave.c
   communication.c
+  moteur.c
+  Servomoteur.c
 )
 
 target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/)

Servomoteur.c

@@ -0,0 +1,88 @@
+#include "moteur.h"
+
+// Define pins servo
+#define SERVO1_PIN 20
+#define SERVO2_PIN 21
+
+// S0 : 26 - PWM 5 - Chan A
+// S1 : 22 - PWM 3 - Chan A
+// S2 : 21 - PWM 2 - Chan B
+// S3 : 20 - PWM 2 - Chan A
+// S4 : 27 - PWM 5 - Chan B
+
+void Servomoteur_init(){
+    // Set wrap of pwm slice
+	pwm_set_wrap(2, 25000);
+    pwm_set_wrap(3, 25000);
+    pwm_set_wrap(5, 25000);
+
+	// Set clock div for this pwm slice
+	pwm_set_clkdiv(2, 100);
+    pwm_set_clkdiv(3, 100);
+    pwm_set_clkdiv(5, 100);
+
+	// Enable pwm slice
+	pwm_set_enabled(2, true);
+    pwm_set_enabled(3, true);
+    pwm_set_enabled(5, true);
+
+	// Set channel level
+	pwm_set_chan_level(2, PWM_CHAN_A, 2800); // S3 - Ouvert : 1700 - Fermé : 2800
+	pwm_set_chan_level(2, PWM_CHAN_B, 2650); // S2 - Ouvert : 1500 - Fermé : 2650
+    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
+	pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - 1750
+    pwm_set_chan_level(5, PWM_CHAN_B, 1500); // S4 - Ouvert 2700 - Fermé : 1500 
+
+
+    // Init pin for the servos
+	gpio_init(SERVO1_PIN);
+	gpio_init(SERVO2_PIN);
+    gpio_init(22);
+    gpio_init(26);
+	gpio_init(27);
+	gpio_set_function(SERVO1_PIN, GPIO_FUNC_PWM);
+	gpio_set_function(SERVO2_PIN, GPIO_FUNC_PWM);
+	gpio_set_function(22, GPIO_FUNC_PWM);
+	gpio_set_function(26, GPIO_FUNC_PWM);
+	gpio_set_function(27, GPIO_FUNC_PWM);
+
+	// 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_pince_tient(){
+    pwm_set_chan_level(2, PWM_CHAN_A, 2800);
+    pwm_set_chan_level(5, PWM_CHAN_B, 1350);
+    pwm_set_chan_level(2, PWM_CHAN_B, 2650);
+}
+
+void Servo_pince_lache(){
+    pwm_set_chan_level(2, PWM_CHAN_A, 1700);
+    pwm_set_chan_level(5, PWM_CHAN_B, 2550);
+    pwm_set_chan_level(2, PWM_CHAN_B, 1500);
+}
+
+void Servo_came_pousse(){
+    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
+}
+
+void Servo_came_neutre(){
+    pwm_set_chan_level(3, PWM_CHAN_A, 1500); // S1 - 2300 Pousse, 1500 Neutre
+}
+
+void Servo_deplie_banderole(){
+    pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - neutre 1750
+}
+
+void Servo_plie_banderole(){
+    pwm_set_chan_level(5, PWM_CHAN_A, 1750); // S0 - Dépose 900 - neutre 1750
+}

Servomoteur.h

@@ -0,0 +1,9 @@
+void Servomoteur_init();
+void Servo_pince_tient();
+void Servo_pince_lache();
+
+void Servo_came_pousse();
+void Servo_came_neutre();
+
+void Servo_deplie_banderole();
+void Servo_plie_banderole();

communication.c

@@ -97,11 +97,11 @@ void communication_init(void){
     i2c_maitre_init();
 }
 
-void communication_envoyer_message(unsigned char * message, unsigned int message_length){
+void communication_send_message(unsigned char * message, unsigned int message_length){
     memcpy(context.mem, message, message_length);
 }
 
-int communication_lire_message(unsigned char * message){
-    i2c_lire_registre(I2C_SLAVE_ADDRESS, 0, message, 255);
+int communication_read_message(unsigned char * message){
+    i2c_lire_registre(I2C_SLAVE_ADDRESS, 0, message, 25);
 
 }

communication.h

@@ -1,5 +1,5 @@
 #include "i2c_maitre.h"
 
 void communication_init(void);
-void communication_envoyer_message(unsigned char * message, unsigned int message_length);
-enum i2c_resultat_t communication_lire_message(unsigned char * message);
+void communication_send_message(unsigned char * message, unsigned int message_length);
+enum i2c_resultat_t communication_read_message(unsigned char * message);

i2c_maitre.c

@@ -31,7 +31,7 @@ enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission,
 
 void i2c_maitre_init(void){
     //stdio_init_all();
-    i2c_init(i2c1, 100 * 1000);
+    i2c_init(i2c1, 10 * 1000);
 
     printf("Initialisation des broches\n");
     for(int i=0; i++; i<=28){

main.c

@@ -1,173 +1,101 @@
- /*****
- * Copyright (c) 2023 - Poivron Robotique
- *
- * SPDX-License-Identifier: BSD-3-Clause
-*/
-#include "communication.h"
 #include "pico/stdlib.h"
-#include "hardware/pwm.h"
-#include <stdio.h>
 #include "hardware/adc.h"
-#define LED_VERTE 25
-#define PIN_VITESSE_M1 2
-#define PIN_SENS_A_M1 0
-#define PIN_SENS_B_M1 1
+#include "communication.h"
+#include "Servomoteur.h"
+#include <stdio.h>
+#include "moteur.h"
 
-int result0, result1;
-int joystic_clicker;
-int pince;
-int ascenceur;
+// Juste pour information - les broches sont re-définies dans i2c_slave
+#define I2C0_SDA_PIN 16
+#define I2C0_SCL_PIN 17
 
+// Juste pour information - les broches sont re-définies dans i2c_master
+#define I2C1_SDA_PIN 18
+#define I2C1_SCL_PIN 19
 
-int M1_INITIALISE()
+// Allumage de la led verte
+#define PIN_LED_VERTE 25
+
+void main(void)
 {
-	gpio_init(PIN_VITESSE_M1);
-	gpio_init(PIN_SENS_A_M1);
-	gpio_init(PIN_SENS_B_M1);
-
-    gpio_set_dir(PIN_VITESSE_M1, GPIO_OUT);
-	gpio_set_dir(PIN_SENS_A_M1, GPIO_OUT);
-	gpio_set_dir(PIN_SENS_B_M1, GPIO_OUT);
-
-	gpio_set_function(PIN_VITESSE_M1, GPIO_FUNC_PWM);
-	pwm_set_wrap(PIN_VITESSE_M1, 100);
-	pwm_set_chan_level(PIN_VITESSE_M1, PWM_CHAN_B, 50);
-    
-}
-
-int M1_AVANCE()
-{
-
-	gpio_put(PIN_VITESSE_M1, 1);
-	gpio_put(PIN_SENS_A_M1, 1);
-	gpio_put(PIN_SENS_B_M1, 0);
-
-
-
-}
-
-int M1_RECULE()
-{
-
-	gpio_put(PIN_VITESSE_M1, 1);
-	gpio_put(PIN_SENS_A_M1, 0);
-	gpio_put(PIN_SENS_B_M1, 1);
-
-
-
-}
-int Adc_Init()
-{
-	adc_init();
-	adc_gpio_init(26);
-	adc_gpio_init(27);
-}
-int AdcRead0()
-{
-	adc_select_input(0);
-	uint16_t resultadc = adc_read();
-	return resultadc;
-}
-
-
-
-
-
-int AdcRead1()
-{
-	adc_select_input(1);
-	uint16_t resultadc = adc_read();
-	return resultadc;
-}
-
-
-
-
-
-
-void main()
-{
-	char message [256];
-	stdio_init_all();
-
-	// Communication
-	communication_init();
-
-	// Pour envoyer un message
-	// communication_envoyer_message(message, 254);
-
-
-	// CLignottement LED
-	gpio_set_function(LED_VERTE, GPIO_FUNC_PWM);
-	pwm_set_wrap(4, 100);
-	pwm_set_chan_level(4, PWM_CHAN_B, 25);
-	pwm_set_enabled(4, true);
-
-	// ADC
-	adc_init();
-	adc_gpio_init(26);
-	adc_gpio_init(27);
+	char message[256], reception[256];
 	
-	while (1) 
-	{
-		// Voie X
-		result1 = AdcRead1();
-	 	
-		message[0] = result1/16;
-		
-		// Voie Y
-		result0 = AdcRead0();
-		
-		message[1] = result0/16;
-		
-		//clic sur le joystic
-		gpio_init(1);
-		gpio_pull_up(1);
-    	gpio_set_dir(1, GPIO_IN);
-		joystic_clicker = gpio_get(1);
-		if (!joystic_clicker)
+	// Init "all"
+    stdio_init_all();
+	Init_motion_motor();
+	Servomoteur_init();
+	communication_init();
+	
+	gpio_init(PIN_LED_VERTE);
+	gpio_set_function(PIN_LED_VERTE, GPIO_FUNC_PWM);
+	pwm_set_clkdiv(4, 100);
+	pwm_set_wrap(4, 100);
+	pwm_set_enabled(4, true);
+	pwm_set_chan_level(4, PWM_CHAN_B, 100);
+
+	int vitesse_angle = 0;
+	int vitesse_m1 = 0;
+	int vitesse_m2 = 0;
+	int vitesse_m3 = 0;
+	int vitesse_m4 = 0;
+
+	while(1){
+
+		if (!(communication_read_message(reception) == I2C_ECHEC))
 		{
-			message[2] = result1/16;
-			message[1] = 128;
-			message[0] = 128;
+			vitesse_m1 = reception[0] - 128;
+			vitesse_m2 = reception[0] - 128;
+			vitesse_m3 = reception[1] - 128;
+			vitesse_m4 = reception[1] - 128;
+
+			vitesse_angle = reception[2] - 128;
+
+			switch(reception[3]){
+				case 0: Servo_pince_lache(); break;
+				case 1: Servo_pince_tient(); break;
+			}
+
+			switch(reception[4]){
+				case 0: Servo_plie_banderole(); break;
+				case 1: Servo_deplie_banderole(); break;
+			}
+			switch(reception[5]){
+				case 0: Servo_came_neutre(); break;
+				case 1: Servo_came_pousse(); break;
+				
+			}
+
 		}
 		else
 		{
-			message[2] = 128;
+			vitesse_angle = 0;
+			vitesse_m1 = 0;
+			vitesse_m2 = 0;
+			vitesse_m3 = 0;
+			vitesse_m4 = 0;
+
+			pwm_set_chan_level(4, PWM_CHAN_B, 10);
+		
 		}
 
+		if(vitesse_angle != 0)
+		{
+			vitesse_m1 = vitesse_angle;
+			vitesse_m2 = -vitesse_angle;
+			vitesse_m3 = vitesse_angle;
+			vitesse_m4 = -vitesse_angle;
+		}
 
+		printf(">vitesseM1:%d\n", vitesse_m1);
+		printf(">vitesseM2:%d\n", vitesse_m2);
+		printf(">vitesseM3:%d\n", vitesse_m3);
+		printf(">vitesseM4:%d\n", vitesse_m4);
 
-		//pince
-		gpio_init(2);
-		gpio_set_dir(2, GPIO_IN);
-		gpio_pull_up(2);
-		pince = gpio_get(2);
-		message [3] = pince;
+		Motor1_speed(vitesse_m1);
+		Motor2_speed(vitesse_m2);
+		Motor3_speed(vitesse_m3);
+		Motor4_speed(vitesse_m4);
 
-		gpio_init(6);
-		gpio_set_dir(6, GPIO_IN);
-		gpio_pull_up(6);
-		ascenceur = gpio_get(6);
-		message[4] = ascenceur;
-
-
-		printf(">x:%d\n", message[0]);
-		printf(">Y:%d\n", message[1]);
-		printf(">Rz:%d\n", message[2]);
-		printf(">pince:%d\n", message[3]);
-		printf(">ascenceur:%d\n", message[4]);
-		sleep_ms(100);
-		communication_envoyer_message(message, 254);
+		
 	}
-	M1_INITIALISE();
-	while(1)
-	{
-		M1_AVANCE();
-		sleep_ms(1000);
-		M1_RECULE();
-		sleep_ms(1000);
-	
-	}
-
-}
+}

moteur.c

@@ -0,0 +1,200 @@
+#include "moteur.h"
+
+// Define pins servo
+#define MOTEUR1_PIN_SENS1 4
+#define MOTEUR1_PIN_SENS2 5
+#define MOTEUR1_PIN_ACTIVATION 0
+#define MOTEUR2_PIN_SENS1 6
+#define MOTEUR2_PIN_SENS2 7
+#define MOTEUR2_PIN_ACTIVATION 1
+#define MOTEUR3_PIN_SENS1 11
+#define MOTEUR3_PIN_SENS2 10
+#define MOTEUR3_PIN_ACTIVATION 2
+#define MOTEUR4_PIN_SENS1 8
+#define MOTEUR4_PIN_SENS2 9
+#define MOTEUR4_PIN_ACTIVATION 3
+
+// 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);
+	gpio_init(MOTEUR2_PIN_SENS1);
+	gpio_init(MOTEUR2_PIN_SENS2);
+	gpio_init(MOTEUR3_PIN_SENS1);
+	gpio_init(MOTEUR3_PIN_SENS2);
+	gpio_init(MOTEUR4_PIN_SENS1);
+	gpio_init(MOTEUR4_PIN_SENS2);
+	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR3_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR3_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR4_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR4_PIN_SENS2, GPIO_OUT);
+
+	// Set direction to 0 (disactivate)
+	gpio_put(MOTEUR1_PIN_SENS1, 0);
+	gpio_put(MOTEUR1_PIN_SENS2, 0);
+	gpio_put(MOTEUR2_PIN_SENS1, 0);
+	gpio_put(MOTEUR2_PIN_SENS2, 0);
+	gpio_put(MOTEUR3_PIN_SENS1, 0);
+	gpio_put(MOTEUR3_PIN_SENS2, 0);
+	gpio_put(MOTEUR4_PIN_SENS1, 0);
+	gpio_put(MOTEUR4_PIN_SENS2, 0);
+
+	// Init pwm pins for motion motors
+	gpio_init(MOTEUR1_PIN_ACTIVATION);
+	gpio_init(MOTEUR2_PIN_ACTIVATION);
+	gpio_init(MOTEUR3_PIN_ACTIVATION);
+	gpio_init(MOTEUR4_PIN_ACTIVATION);
+	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR3_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR4_PIN_ACTIVATION, GPIO_FUNC_PWM);
+
+	// Set wrap of pwm slices
+	pwm_set_wrap(0, 127);
+	pwm_set_wrap(1, 127);
+
+	// Active all pwm slices
+	pwm_set_enabled(0, true);
+	pwm_set_enabled(1, true);
+
+	// Set speed to 0
+	pwm_set_chan_level(0, PWM_CHAN_A, 0);
+	pwm_set_chan_level(0, PWM_CHAN_B, 0);
+	pwm_set_chan_level(1, PWM_CHAN_A, 0);
+	pwm_set_chan_level(1, PWM_CHAN_B, 0);
+
+	
+
+	
+}
+
+
+// Set motor 1 speed forward
+void Motor1_forward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR1_PIN_SENS1, 0);
+	gpio_put(MOTEUR1_PIN_SENS2, 1);
+}
+
+// Set motor 1 speed backward
+void Motor1_backward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR1_PIN_SENS1, 1);
+	gpio_put(MOTEUR1_PIN_SENS2, 0);
+}
+
+// Set motor 1 speed and direction (negative value : backward / positive value : forward)
+void Motor1_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor1_backward(speed);
+	}
+	else
+	{
+		Motor1_forward(speed);
+	}
+}
+
+// Set motor 2 speed forward
+void Motor2_forward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR2_PIN_SENS1, 0);
+	gpio_put(MOTEUR2_PIN_SENS2, 1);
+}
+
+// Set motor 2 speed backward
+void Motor2_backward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR2_PIN_SENS1, 1);
+	gpio_put(MOTEUR2_PIN_SENS2, 0);
+}
+
+// Set motor 2 speed and direction (negative value : backward / positive value : forward)
+void Motor2_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor2_backward(speed);
+	}
+	else
+	{
+		Motor2_forward(speed);
+	}
+}
+
+// Set motor 3 speed forward
+void Motor3_forward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR3_PIN_SENS1, 0);
+	gpio_put(MOTEUR3_PIN_SENS2, 1);
+}
+
+// Set motor 3 speed backward
+void Motor3_backward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR3_PIN_SENS1, 1);
+	gpio_put(MOTEUR3_PIN_SENS2, 0);
+}
+
+// Set motor 3 speed and direction (negative value : backward / positive value : forward)
+void Motor3_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor3_backward(speed);
+	}
+	else
+	{
+		Motor3_forward(speed);
+	}
+}
+
+// Set motor 4 speed forward
+void Motor4_forward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR4_PIN_SENS1, 1);
+	gpio_put(MOTEUR4_PIN_SENS2, 0);
+}
+
+// Set motor 4 speed backward
+void Motor4_backward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR4_PIN_SENS1, 0);
+	gpio_put(MOTEUR4_PIN_SENS2, 1);
+}
+
+// Set motor 4 speed and direction (negative value : backward / positive value : forward)
+void Motor4_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor4_backward(speed);
+	}
+	else
+	{
+		Motor4_forward(speed);
+	}
+}
+
+// Set

moteur.h

@@ -0,0 +1,17 @@
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+
+void Init_motion_motor(void);
+void Motor1_forward(int speed);
+void Motor1_backward(int speed);
+void Motor1_speed(int speed);
+void Motor2_forward(int speed);
+void Motor2_backward(int speed);
+void Motor2_speed(int speed);
+void Motor3_forward(int speed);
+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);
+