ToF, Gyro, Bluetooth, OpenMV working ! Bluetooth Baud = 230400

added execution speed measure function

source-code/LICRAWM 1.5.4 - Software/main.ino

@@ -4,6 +4,7 @@
 #include <VL53L0X.h>
 #include <MPU6050_tockn.h>
 
+
 _led LED5(LED_5);
 _led LED1(LED_1);
 _led LED3(LED_3);
@@ -22,27 +23,40 @@ MPU6050 mpu6050(Wire);
 
 
 void setup() {
-  Serial2.begin(9600);
+  Serial2.begin(230400);
+  //Serial2.setTimeout(100);
   Serial3.begin(9600);
   boot();
   Wire.begin();
-  LED2.on();
+  LED1.on();
 
   boot_gyro();
-  LED4.on(); //booting gyro done!
+  LED3.on(); //booting gyro done!
   boot_tof();
-  LED1.on(); //booting tof done!
+  LED5.on(); //booting tof done!
 
 }
 
 void loop(){
 
-  bluetooth_input_check(); 
-  openmv_digital_decode();
-
-  print_tof_reading();
-
-  mpu6050.update(); 
-  print_gyro_reading();
+  unsigned long start = micros();
+
+
+    //TraceFunc();
+    bluetooth_input_check(); 
+    //openmv_digital_decode();
+
+    get_tof_reading();
+
+    get_gyro_reading();
+
+  unsigned long end = micros();
+  unsigned long delta = end - start;
+
+  if(DEBUG_SPEED){
+    Serial2.print("\n -> Loop ran in ");
+    Serial2.print(delta);
+    Serial2.print("ns \n");
+  }
 
 }