Fin probable projet

REste le problèmes des LEDs après que l'epuck soit tombé alors qu'elles fonctionnaient bien avant

check_collision.c

@@ -7,30 +7,71 @@
 
 #include <sensors/proximity.h>
 #include <ch.h>
+#include <hal.h>
+#include <chprintf.h>
 #include <motors.h>
+#include <leds.h>
 #include "check_collision.h"
 
+#define DETECTION_THRESHOLD	100
+/*
+ * wall_detection = 0 -> nothing changes
+ * wall_detection = 1 -> wall detected in front
+ * wall_detection = 2 -> wall detected in the back
+ */
+static uint8_t wall_detection = 0;
+
 // thread that detect a wall via infra-red captor
-static THD_WORKING_AREA(waCheckCollision, 64); //Value of the stack has to be defined later on.
+static THD_WORKING_AREA(waCheckCollision, 256); //Value of the stack has to be defined later on.
 static THD_FUNCTION(CheckCollision, arg) {
 
     chRegSetThreadName(__FUNCTION__);
     (void)arg;
 
-    uint16_t proxi = 0;
+    int proxi_values[8] = {0, 0, 0, 0, 0, 0, 0, 0};
 
     while(1) {
+    	// Stocks the values in array
+    	for(uint8_t i = 0 ; i < 8 ; ++i){
+    		proxi_values[i] = get_calibrated_prox(i);
+    	}
 
-    	//proxi = get_calibrated_prox();
 
-    	//if(proxi >= 100){
-    	//	right_motor_set_speed(0);
-    	//	left_motor_set_speed(0);
-    	//
-    	//}
+    	uint8_t check_sensors = 0;
+    	for(uint8_t i = 0 ; i < 8 ; ++i){
+    		// Checks if no walls
+    		if(proxi_values[i] < DETECTION_THRESHOLD){
+    			if(check_sensors == 7){
+    				wall_detection = 0;
+    				clear_leds();
+    				check_sensors = 0;
+    			}else{
+        			check_sensors += 1;
+    			}
+    		// Walls detection
+    		}else if( (i == 0) | (i == 1) | (i == 6) | (i == 7) ){
+    			wall_detection = 1;
+    			//Toggle LED
+    			clear_leds();
+    			set_led(LED1, 1);
+    			set_rgb_led(LED2, 0, 0, 255);
+    			set_rgb_led(LED8, 0, 0, 255);
+    		}else if( (i == 3) | (i == 4) ){
+    			wall_detection = 2;
 
-    	// Check each 500ms if there is a wall or not
-    	chThdSleepMilliseconds(1000);
+				// Toggle LED
+				clear_leds();
+				set_led(LED5, 1);
+				set_rgb_led(LED4, 0, 0, 255);
+				set_rgb_led(LED6, 0, 0, 255);
+    		}else{
+				wall_detection = 0;
+				clear_leds();
+			}
+    	}
+
+    	// Check each 50ms if there is a wall or not
+    	chThdSleepMilliseconds(50);
     }
 }
 
@@ -38,3 +79,7 @@ static THD_FUNCTION(CheckCollision, arg) {
 void check_collision_start(void){
 	chThdCreateStatic(waCheckCollision, sizeof(waCheckCollision), NORMALPRIO+1, CheckCollision, NULL);
 }
+
+uint8_t get_wall_detection(void){
+	return wall_detection;
+}

check_collision.h

@@ -11,5 +11,7 @@
 // Function to create the threat that check if there is a wall or not (high prio)
 void check_collision_start(void);
 
+// Getter of wall_detection
+uint8_t get_wall_detection(void);
 
 #endif /* CHECK_COLLISION_H_ */

compute_case.c

@@ -17,17 +17,18 @@
 #define		GRAVITY		9.80665f
 
 static float acc_x = 0;
+static float acc_z = 0;
 static uint8_t 	acc_case = 0;
 
-// Thread that tells us in which case (0, I, II, III, IV) (spqr) we are
+// Thread that tells us in which case (0, I, II, III, IV) (SPQR) we are
 static THD_WORKING_AREA(waSelectCase, 128); //Value of the stack has to be defined later on.
 static THD_FUNCTION(SelectCase, arg) {
 
     chRegSetThreadName(__FUNCTION__);
     (void)arg;
 
+
     float acc_y = 0;
-    float acc_z = 0;
 
     while(1){
     	// get x and y component accelerometer

main.c

@@ -6,20 +6,24 @@
 #include <hal.h>
 #include <memory_protection.h>
 #include <usbcfg.h>
-#include <main.h>
 #include <chprintf.h>
 #include <motors.h>
 #include <arm_math.h>
 #include <sensors/imu.h>
 #include <sensors/proximity.h>
+#include <leds.h>
+#include <spi_comm.h>
+#include <msgbus/messagebus.h>
+#include <i2c_bus.h>
 
 // project files includes
+#include "main.h"
 #include "compute_case.h"
-#include "pid_regulator.h"
 #include "check_collision.h"
+#include "motor_speed.h"
 
 messagebus_t bus;
-MUTEX_DECL(bus_lock);
+MUTEX_DECL(bus_lock); // @suppress("Field cannot be resolved")
 CONDVAR_DECL(bus_condvar);
 
 static void serial_start(void)
@@ -44,10 +48,7 @@ int main(void)
     /** Inits the Inter Process Communication bus. */
     messagebus_init(&bus, &bus_lock, &bus_condvar);
 
-
-
-
-    // Start the imu
+    // Starts the imu
     imu_start();
     // Inits the motors
     motors_init();
@@ -55,7 +56,9 @@ int main(void)
     serial_start();
     // Starts the IR proximity sensor
     proximity_start();
-    //starts the USB communication
+    // Starts the SPI for the RGB LED
+    spi_comm_start();
+    // Starts the USB communication
     //usb_start();
 
 
@@ -71,13 +74,19 @@ int main(void)
     // Start thread
     check_collision_start();
     select_case_start();
-    pid_regulator_start();
-
+    motor_speed_start();
+
+    // LED indicates that the callibration is completed
+    for(uint8_t i = 0 ; i < 3; ++i){
+    	set_front_led(1);
+    	chThdSleepMilliseconds(100);
+    	set_front_led(0);
+    	chThdSleepMilliseconds(100);
+    }
     /* Infinite loop. */
     while (1) {
-    	palTogglePad(GPIOD, GPIOD_LED_FRONT);
-    	chprintf((BaseSequentialStream *)&SD3, "AccX = %f \n",get_acc_x());
-    	chprintf((BaseSequentialStream *)&SD3, "Case = %d \n",get_acc_case());
+    	//chprintf((BaseSequentialStream *)&SD3, "AccX = %f \n",get_acc_x());
+    	//chprintf((BaseSequentialStream *)&SD3, "Case = %d \n",get_acc_case());
     	//chprintf((BaseSequentialStream *)&SD3, "IR2 = %d \n",get_calibrated_prox(1));
     	//chprintf((BaseSequentialStream *)&SD3, "IR1 = %d \n",get_calibrated_prox(0));
     	//chprintf((BaseSequentialStream *)&SD3, "IR7 = %d \n",get_calibrated_prox(6));

makefile

@@ -11,7 +11,7 @@ GLOBAL_PATH = ../../lib/e-puck2_main-processor
 #Source files to include
 CSRC += ./main.c \
 		./compute_case.c \
-		./pid_regulator.c \
+		./motor_speed.c \
 		./check_collision.c \
 
 #Header folders to include

pid_regulator.c → motor_speed.c

 /*
- * pid_regulator.c
+ * motor_speed.c
  *
  *  Created on: 14 avr. 2022
  *      Author: Corentin Jossi
  */
+#include "motor_speed.h"
+
 #include <ch.h>
 #include <hal.h>
 #include <math.h>
 #include <motors.h>
 #include <selector.h>
-#include <chprintf.h>
 #include "compute_case.h"
-#include "pid_regulator.h"
+#include "check_collision.h"
 
 
 #define	KP				500.0f
@@ -22,9 +23,14 @@
 #define MIN_ERROR		2.0f
 #define GOAL			0
 
+// Declaration of function
+void pid_regulator(	float *error, float *derivative, float *last_error, float *correction_speed,
+					float *straight_speed, float *sum_error);
+void stop_motor(void);
+
 // The PID regulator thread
-static THD_WORKING_AREA(waPIDRegulator, 256); //Value of the stack has to be defined later on.
-static THD_FUNCTION(PIDRegulator, arg) {
+static THD_WORKING_AREA(waMotorSpeed, 256); //Value of the stack has to be defined later on.
+static THD_FUNCTION(MotorSpeed, arg) {
 
     chRegSetThreadName(__FUNCTION__);
     (void)arg;
@@ -44,54 +50,40 @@ static THD_FUNCTION(PIDRegulator, arg) {
 
 		//----------PID regulator----------
 
-		error = get_acc_x() - GOAL;
-		derivative = error - last_error;
-
-		// Set threshold
-		if(fabs(error) <= ERROR_THRESHOLD){
-			error = 0;
-			sum_error = 0;
-			last_error = 0;
-		}
-
-		sum_error += error;
-
-		//we set a maximum and a minimum for the sum to avoid an uncontrolled growth
-		if(sum_error > MAX_SUM_ERROR){
-			sum_error = MAX_SUM_ERROR;
-		}else if(sum_error < -MAX_SUM_ERROR){
-			sum_error = -MAX_SUM_ERROR;
-		}
-
-		correction_speed = KP*error + KD*derivative +  KI*sum_error;
+		pid_regulator( &error, &derivative, &last_error, &correction_speed,
+					   &straight_speed, &sum_error);
 
+		//--------------Motors and other things----------------------
 
-		//we set a minimum for the error for the X axis to know when to start rolling forwards and backwards
-		if((error < MIN_ERROR) | (error > -MIN_ERROR)) {
-			// 40 percent of max speed
-			straight_speed = MOTOR_SPEED_LIMIT*0.5;
-		}else{
-			straight_speed = 0;
+		// Check if the motor speed is greater than the limit
+		// straight_speed + correction_speed = MOTOR_SPEED_LIMIT
+		// straight_speed - correction_speed stays the same
+		if(straight_speed+correction_speed > MOTOR_SPEED_LIMIT){
+			correction_speed = (MOTOR_SPEED_LIMIT-straight_speed+correction_speed)/2;
+			straight_speed = MOTOR_SPEED_LIMIT-correction_speed;
 		}
 
-		last_error = error;
-
-		//--------------Motors and other things----------------------
-
 		// Speed to the motor by cases
 		if( (get_acc_case() == 0) | (get_selector() == 0) ){
-			right_motor_set_speed(0);
-			left_motor_set_speed(0);
+			stop_motor();
 		}else if( (get_acc_case() == 1) | (get_acc_case() == 3) ) {
 			// Case I and III
 			// Front on top
-			right_motor_set_speed((int16_t)(straight_speed-correction_speed));
-			left_motor_set_speed((int16_t)(straight_speed+correction_speed));
+			if(get_wall_detection() == 1){
+				stop_motor();
+			}else{
+				right_motor_set_speed((int16_t)(straight_speed-correction_speed));
+				left_motor_set_speed((int16_t)(straight_speed+correction_speed));
+			}
 		}else{
 			// Case II and IV
 			// Back on top
-			right_motor_set_speed((int16_t)(-straight_speed+correction_speed));
-			left_motor_set_speed((int16_t)(-straight_speed-correction_speed));
+			if(get_wall_detection() == 2){
+				stop_motor();
+			}else{
+				right_motor_set_speed((int16_t)(-(straight_speed-correction_speed)));
+				left_motor_set_speed((int16_t)(-(straight_speed+correction_speed)));
+			}
 		}
 
 		//25Hz
@@ -100,7 +92,48 @@ static THD_FUNCTION(PIDRegulator, arg) {
 }
 
 
-// Function to create the thread PIDRegulator
-void pid_regulator_start(void){
-	chThdCreateStatic(waPIDRegulator, sizeof(waPIDRegulator), NORMALPRIO, PIDRegulator, NULL);
+// Function to create the thread MotorSpeed
+void motor_speed_start(void){
+	chThdCreateStatic(waMotorSpeed, sizeof(waMotorSpeed), NORMALPRIO, MotorSpeed, NULL);
+}
+
+// Function of the PID regulator
+void pid_regulator(	float *error, float *derivative, float *last_error, float *correction_speed,
+					float *straight_speed, float *sum_error){
+
+	*error = get_acc_x() - GOAL;
+	*derivative = *error - *last_error;
+
+	// Set threshold
+	if(fabs(*error) <= ERROR_THRESHOLD){
+		*error = 0;
+		*sum_error = 0;
+		*last_error = 0;
+	}
+
+	*sum_error += *error;
+
+	//we set a maximum and a minimum for the sum to avoid an uncontrolled growth
+	if(*sum_error > MAX_SUM_ERROR){
+		*sum_error = MAX_SUM_ERROR;
+	}else if(*sum_error < -MAX_SUM_ERROR){
+		*sum_error = -MAX_SUM_ERROR;
+	}
+
+	*correction_speed = KP * (*error) + KD * (*derivative) +  KI * (*sum_error);
+
+
+	//we set a minimum for the error for the X axis to know when to start rolling forwards and backwards
+	if((*error < MIN_ERROR) | (*error > -MIN_ERROR)) {
+		*straight_speed = MOTOR_SPEED_LIMIT*0.7;
+	}else{
+		*straight_speed = 0;
+	}
+
+	*last_error = *error;
+}
+
+void stop_motor(void){
+	right_motor_set_speed(0);
+	left_motor_set_speed(0);
 }

motor_speed.h

+/*
+ * motor_speed.h
+ *
+ *  Created on: 14 avr. 2022
+ *      Author: Corentin Jossi
+ */
+
+#ifndef MOTOR_SPEED_H_
+#define MOTOR_SPEED_H_
+
+// Function to create the thread MotorSpeed
+void motor_speed_start(void);
+
+
+#endif /* MOTOR_SPEED_H_ */

pid_regulator.h

-/*
- * pid_regulator.h
- *
- *  Created on: 14 avr. 2022
- *      Author: Corentin Jossi
- */
-
-#ifndef PID_REGULATOR_H_
-#define PID_REGULATOR_H_
-
-// Function to create the thread PIDRegulator
-void pid_regulator_start(void);
-
-
-#endif /* PID_REGULATOR_H_ */