diff --git a/check_collision.c b/check_collision.c
index 79f52afd05be2d2350fc3c69700840c1a1bcb594..c76c60192f42f182bfa4cc299aaf16f759161577 100644
--- a/check_collision.c
+++ b/check_collision.c
@@ -54,16 +54,16 @@ static THD_FUNCTION(CheckCollision, arg) {
     			//Toggle LED
     			clear_leds();
     			set_led(LED1, 1);
-    			set_rgb_led(LED2, 0, 0, 255);
-    			set_rgb_led(LED8, 0, 0, 255);
+    			set_rgb_led(LED2, 255, 255, 255);
+    			set_rgb_led(LED8, 255, 255, 255);
     		}else if( (i == 3) | (i == 4) ){
     			wall_detection = 2;
 
 				// Toggle LED
 				clear_leds();
 				set_led(LED5, 1);
-				set_rgb_led(LED4, 0, 0, 255);
-				set_rgb_led(LED6, 0, 0, 255);
+				set_rgb_led(LED4, 255, 255, 255);
+				set_rgb_led(LED6, 255, 255, 255);
     		}else{
 				wall_detection = 0;
 				clear_leds();
diff --git a/main.c b/main.c
index d22c348bcd8e5092b44b519ce70509d7e65779ac..a6d699a916cda7802ef30811222a83cdfbde93af 100644
--- a/main.c
+++ b/main.c
@@ -85,12 +85,6 @@ int main(void)
     }
     /* Infinite loop. */
     while (1) {
-    	//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));
-    	//chprintf((BaseSequentialStream *)&SD3, "IR8 = %d \n",get_calibrated_prox(7));
 
     	chThdSleepMilliseconds(1000);
     }
diff --git a/motor_speed.c b/motor_speed.c
index 43a1c9976aba7faffa1eac6ab038b17c32e61d82..817c9755480b076b0466ffe7d23bb1a4b69a931d 100644
--- a/motor_speed.c
+++ b/motor_speed.c
@@ -37,7 +37,6 @@ static THD_FUNCTION(MotorSpeed, arg) {
 
     systime_t time;
 
-    //uint8_t  up_down = 0;
     float error = 0;
     float derivative = 0;
     float last_error = 0;