diff --git a/main.c b/main.c
index 396efd3..3e308b3 100644
--- a/main.c
+++ b/main.c
@@ -3,94 +3,134 @@
  *
  * SPDX-License-Identifier: BSD-3-Clause
 */
+// Include libraries
 #include "pico/stdlib.h"
 #include "hardware/pwm.h"
 #include "hardware/adc.h"
 #include <stdio.h>
 
-#define MOTEUR1_PIN_SENS1 2
-#define MOTEUR1_PIN_SENS2 3
+// Define pins
+#define MOTEUR1_PIN_SENS1 4
+#define MOTEUR1_PIN_SENS2 5
 #define MOTEUR1_PIN_ACTIVATION 0
-
-#define MOTEUR2_PIN_SENS1 4
-#define MOTEUR2_PIN_SENS2 5
+#define MOTEUR2_PIN_SENS1 6
+#define MOTEUR2_PIN_SENS2 7
 #define MOTEUR2_PIN_ACTIVATION 1
+#define MOTEUR3_PIN_SENS1 8
+#define MOTEUR3_PIN_SENS2 9
+#define MOTEUR3_PIN_ACTIVATION 2
+#define MOTEUR4_PIN_SENS1 10
+#define MOTEUR4_PIN_SENS2 11
+#define MOTEUR4_PIN_ACTIVATION 3
 
-int sens_moteur1 = 0;
-int sens_moteur2 = 0;
-
-void upload_motor(void);
+// Init all motion motors pins
+void Init_motion_motor(void);
+// Set motor 1 speed forward
+void Motor1_forward(int speed);
+// Set motor 1 speed backward
+void Motor1_backward(int speed);
+// Set motor 1 speed and direction (negative value : backward / positive value : forward)
+void Motor1_speed(int speed);
 
 void main(void)
 {
     stdio_init_all();
-	
-	gpio_init(MOTEUR1_PIN_SENS1);
-	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
-	gpio_init(MOTEUR1_PIN_SENS2);
-	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
-	gpio_init(MOTEUR2_PIN_SENS1);
-	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
-	gpio_init(MOTEUR2_PIN_SENS2);
-	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
 
-	gpio_init(MOTEUR1_PIN_ACTIVATION);
-	gpio_init(MOTEUR2_PIN_ACTIVATION);
+	Init_motion_motor();
 
-	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
-	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
-
-	pwm_set_wrap(0, 1000);
-	pwm_set_chan_level(0, PWM_CHAN_A, 800);
-
-	pwm_set_enabled(0, true);
-
-	sens_moteur1 = 1;
-	sens_moteur2 = 1;
-
-	upload_motor();
-
-	while(1){
-		tight_loop_contents();
+	while(1)
+	{
+		Motor1_speed(-200);
+		sleep_ms(3000);
+		Motor1_speed(500);
+		sleep_ms(3000);
 	}
+
 }
 
-void upload_motor(void)
+// Init all motion motors pins
+void Init_motion_motor(void)
 {
-	switch(sens_moteur1)
+	// 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, 1000);
+	pwm_set_wrap(1, 1000);
+
+	// 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)
 	{
-	case 0:
-		gpio_put(MOTEUR1_PIN_SENS1, 0);
-		gpio_put(MOTEUR1_PIN_SENS2, 0);
-		break;
-
-	case 1:
-		gpio_put(MOTEUR1_PIN_SENS1, 1);
-		gpio_put(MOTEUR1_PIN_SENS2, 0);
-		break;
-	
-	case 2:
-		gpio_put(MOTEUR1_PIN_SENS1, 0);
-		gpio_put(MOTEUR1_PIN_SENS2, 1);
-		break;
+		speed = -speed;
+		Motor1_backward(speed);
 	}
-	
-	switch(sens_moteur2)
+	else
 	{
-	case 0:
-		gpio_put(MOTEUR2_PIN_SENS1, 0);
-		gpio_put(MOTEUR2_PIN_SENS2, 0);
-		break;
-
-	case 1:
-		gpio_put(MOTEUR2_PIN_SENS1, 1);
-		gpio_put(MOTEUR2_PIN_SENS2, 0);
-		break;
-	
-	case 2:
-		gpio_put(MOTEUR2_PIN_SENS1, 0);
-		gpio_put(MOTEUR2_PIN_SENS2, 1);
-		break;
+		Motor1_forward(speed);
 	}
-
 }
\ No newline at end of file