diff --git a/scripts/connect_bluetooth.py b/scripts/connect_bluetooth.py
index 81c33f41..aed3387a 100644
--- a/scripts/connect_bluetooth.py
+++ b/scripts/connect_bluetooth.py
@@ -184,6 +184,11 @@ def get_battery_voltage(self):
         self.send_bt('battery\0')
         return float(self.wait_filtered()[-1])
 
+    def get_diagnostics_matrix(self):
+        self.filter_next(function='log_diagnostics_matrix')
+        self.send_bt('diagnostics_matrix\0')
+        return float(self.wait_filtered()[-1])
+
     def get_configuration_variables(self):
         self.filter_next(function='log_configuration_variables')
         self.send_bt('configuration_variables\0')
@@ -251,6 +256,10 @@ def do_battery(self, *args):
         """Get battery voltage."""
         print(self.proxy.get_battery_voltage())
 
+    def do_diagnostics_matrix(self, *args):
+        """Get diagnostics matrix."""
+        print(self.proxy.get_diagnostics_matrix())
+
     def do_configuration_variables(self, *args):
         """Get configuration variables."""
         pprint(self.proxy.get_configuration_variables())
diff --git a/src/diagnostics.c b/src/diagnostics.c
new file mode 100644
index 00000000..704b143e
--- /dev/null
+++ b/src/diagnostics.c
@@ -0,0 +1,79 @@
+#include "diagnostics.h"
+
+static volatile float log_matrix[NUM_LOG_ELEMENTS];
+static volatile int log_set_index = -1;
+static volatile int log_get_index = -1;
+static volatile int num_elements_counter;
+static volatile bool enable_diagnostics;
+static volatile bool enable_read_log;
+
+static int inc_log_set_index(void)
+{
+	if (log_set_index == (NUM_LOG_ELEMENTS - 1)) {
+		log_set_index = 0;
+	} else {
+		log_set_index++;
+	}
+	LOG_INFO("%d set_index", log_set_index);
+	return log_set_index;
+}
+
+static int inc_log_get_index(void)
+{
+	if (log_get_index == (NUM_LOG_ELEMENTS - 1)) {
+		log_get_index = 0;
+	} else {
+		log_get_index++;
+	}
+	LOG_INFO("%d get_index", log_get_index);
+	return log_get_index;
+}
+
+bool get_enable_diagnostics(void)
+{
+	return enable_diagnostics;
+}
+
+void set_enable_diagnostics(bool value)
+{
+	enable_diagnostics = value;
+}
+
+int get_log_set_index(void)
+{
+	return log_set_index;
+}
+
+/**
+ * @brief Function to set the sensors distance.
+ */
+void set_log_matrix(void)
+{
+	if (get_enable_diagnostics()) {
+		log_matrix[inc_log_set_index()] = get_front_left_distance();
+	}
+}
+
+/**
+ * @brief Function to get the sensors distance.
+ *
+ *@return The log_matrix
+ */
+float get_log_matrix_element(void)
+{
+	if (num_elements_counter < NUM_LOG_ELEMENTS) {
+		num_elements_counter++;
+		return log_matrix[inc_log_get_index()];
+	}
+	return 0;
+}
+
+/**
+ * @brief Function to reset the buffer.
+ */
+void reset_log_matrix(void)
+{
+	log_set_index = -1;
+	log_get_index = -1;
+	num_elements_counter = 0;
+}
diff --git a/src/diagnostics.h b/src/diagnostics.h
new file mode 100644
index 00000000..3a49ce73
--- /dev/null
+++ b/src/diagnostics.h
@@ -0,0 +1,17 @@
+#ifndef __DIAGNOSTICS_H
+#define __DIAGNOSTICS_H
+
+#include "detection.h"
+#include "formatting.h"
+
+#define NUM_LOG_ELEMENTS 2000
+
+void set_log_matrix(void);
+float get_log_matrix_element(void);
+void set_enable_diagnostics(bool value);
+bool get_enable_diagnostics(void);
+int get_log_set_index(void);
+void reset_log_matrix(void);
+
+
+#endif /* __DIAGNOSTICS_H */
diff --git a/src/logging.c b/src/logging.c
index 7e62df5b..eef2ffb6 100644
--- a/src/logging.c
+++ b/src/logging.c
@@ -254,3 +254,13 @@ void log_gyro_degrees_pub(void)
 {
 	LOG_INFO("PUB,line,gyro_degrees,%f", get_gyro_z_degrees());
 }
+
+/**
+ * @brief Log diagnostic matrix, published for real time.
+ */
+void log_diagnostic_matrix(void)
+{
+	for (int i= 0; i < NUM_LOG_ELEMENTS; i++){
+	LOG_INFO("PUB,line,element,%f", get_log_matrix_element());
+}
+}
diff --git a/src/logging.h b/src/logging.h
index 7980e6fe..79f7f35e 100644
--- a/src/logging.h
+++ b/src/logging.h
@@ -4,6 +4,7 @@
 #include "battery.h"
 #include "control.h"
 #include "detection.h"
+#include "diagnostics.h"
 #include "encoder.h"
 #include "formatting.h"
 #include "move.h"
@@ -26,5 +27,6 @@ void log_walls_detection(void);
 void log_gyro_raw_pub(void);
 void log_gyro_dps_pub(void);
 void log_gyro_degrees_pub(void);
+void log_diagnostic_matrix(void);
 
 #endif /* __LOGGING_H */
diff --git a/src/main.c b/src/main.c
index 5f219d8e..4e768fa3 100644
--- a/src/main.c
+++ b/src/main.c
@@ -2,6 +2,7 @@
 #include "clock.h"
 #include "control.h"
 #include "detection.h"
+#include "diagnostics.h"
 #include "eeprom.h"
 #include "encoder.h"
 #include "hmi.h"
@@ -29,6 +30,7 @@ void sys_tick_handler(void)
 	update_gyro_readings();
 	update_encoder_readings();
 	motor_control();
+	set_log_matrix();
 }
 
 /**
@@ -165,11 +167,24 @@ int main(void)
 	setup();
 	set_speed_mode(0, false);
 	systick_interrupt_enable();
-	competition();
+/*	LOG_INFO("diagnostics disabled, index %d", get_log_set_index());
+	sleep_seconds(1);
+	LOG_INFO("diagnostics enabled, index %d", get_log_set_index());
+	set_enable_diagnostics(true);
+	sleep_seconds(0.01);
+	LOG_INFO("diagnostics disabled, index %d", get_log_set_index());
+	set_enable_diagnostics(false);
+	sleep_seconds(1);
+	LOG_INFO("diagnostics disabled, index %d", get_log_set_index());*/
+	log_diagnostic_matrix();
+	//competition();
 	while (1) {
-		if (button_left_read_consecutive(500))
+	/*	if (button_left_read_consecutive(500))
 			training();
-		execute_commands();
+		execute_commands();*/
+	//	LOG_INFO("%d",get_log_matrix());
+		//sleep_seconds(0.1);
+
 	}
 
 	return 0;
diff --git a/src/serial.c b/src/serial.c
index 86319ab2..7d7b5da8 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -100,6 +100,8 @@ static void process_command(void)
 		run_static_turn_right_profile_signal = true;
 	else if (!strcmp(buffer.data, "run front_sensors_calibration"))
 		run_front_sensors_calibration_signal = true;
+	else if (!strcmp(buffer.data, "get diagnostics_matrix"))
+		log_diagnostic_matrix();
 	else if (starts_with("move "))
 		strcpy(run_movement_sequence_signal, buffer.data);
 	else if (starts_with("set micrometers_per_count "))