tof

Author: gbr1

examples/tof3/tof3.ino

@@ -0,0 +1,48 @@
+#include <Wire.h>
+#include <VL53L1X.h>
+#include <Wire.h>
+
+TwoWire wire(PB7, PB8);
+const uint8_t sensorCount = 3;
+
+const uint8_t xshutPins[sensorCount] = { PC5, PB0, PB1 }; //Right, Center, Left
+
+VL53L1X sensors[sensorCount];
+
+void setup(){
+  while (!Serial);
+  Serial.begin(115200);
+  wire.begin();
+  wire.setClock(400000);
+
+  for (uint8_t i = 0; i < sensorCount; i++){
+    pinMode(xshutPins[i], OUTPUT);
+    digitalWrite(xshutPins[i], LOW);
+  }
+
+  for (uint8_t i = 0; i < sensorCount; i++){
+    pinMode(xshutPins[i], INPUT);
+    delay(10);
+    sensors[i].setBus(&wire);
+    sensors[i].setTimeout(500);
+    if (!sensors[i].init()){
+      Serial.print("Failed to detect and initialize sensor ");
+      Serial.println(i);
+      while (1);
+    }
+
+    sensors[i].setAddress(0x2A + i);
+    sensors[i].startContinuous(50);
+  }
+}
+
+void loop(){
+  for (uint8_t i = 0; i < sensorCount; i++){
+    Serial.print(sensors[i].read());
+    if (sensors[i].timeoutOccurred()){
+        Serial.print(" TIMEOUT");
+    }
+    Serial.print('\t');
+  }
+  Serial.print("\n");
+}

examples/tofM/tofM.ino

@@ -0,0 +1,50 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <vl53l7cx_class.h>
+
+
+TwoWire wire(PB7, PB8);
+#define DEV_I2C wire
+
+#define SerialPort Serial
+
+#define LPN_PIN PB1
+#define I2C_RST_PIN PB0
+
+VL53L7CX sensor_vl53l7cx(&DEV_I2C, LPN_PIN, I2C_RST_PIN);
+
+
+uint8_t res = VL53L7CX_RESOLUTION_8X8; //VL53L7CX_RESOLUTION_4X4
+
+
+void setup(){
+  SerialPort.begin(115200);
+  DEV_I2C.begin();
+  sensor_vl53l7cx.begin();
+  sensor_vl53l7cx.init_sensor();
+  sensor_vl53l7cx.vl53l7cx_set_resolution(res);
+  sensor_vl53l7cx.vl53l7cx_start_ranging();
+}
+
+void loop(){
+  VL53L7CX_ResultsData Results;
+  uint8_t NewDataReady = 0;
+  uint8_t status;
+  do {
+    status = sensor_vl53l7cx.vl53l7cx_check_data_ready(&NewDataReady);
+  } while (!NewDataReady);
+
+  if ((!status) && (NewDataReady != 0)) {
+    status = sensor_vl53l7cx.vl53l7cx_get_ranging_data(&Results);
+    for (int y=0; y<8; y++){
+      for (int x=0; x<8; x++){
+        Serial.print((int)Results.distance_mm[x+y*8]);
+        Serial.print(" ");
+      }
+      Serial.println();
+    }
+    Serial.println();
+  }
+  delay(100);
+
+}

src/Arduino_Robot_Firmware.h

@@ -18,14 +18,16 @@ class Arduino_Robot_Firmware{
         Servo * servo_A;
         Servo * servo_B;
 
+        TwoWire * wire;
+
+
     public:
         RGBled * led1;
         RGBled * led2;
         DCmotor * motor_left;
         DCmotor * motor_right;
         Encoder * encoder_left;
         Encoder * encoder_right;
-        TwoWire * wire;
 
 
         Arduino_Robot_Firmware();