Fix LEDs

Main features are working

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();

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);
     }

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;