Distance&Angle without ROS

VL53L1XtoDistAngle.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+# Beispiel-Skript für den STM TOF-Sensor VL53L1X
+# Grundgeruest stammt aus VL53L1X library for Arduino von Pololu
+# github.com/pololu/vl53l1x-arduino fuer Arduino.
+# Version mit ROI-Zentrum in Richtung lange Seite des Sensors mit nur 4 Winkeln scannen.
+# Da der ROI minimal 4 Pixel breit sein muss, kann dieser um bis zu 13 (überlappenden)
+# Positionen verschoben werden. Ohne Überlappung nur um 4 Position = 4 Scanwinkel.
+# Vom Sensor wird nur die Reichweite abgefragt, damit höhere Messrate.
+# Modul VL53L1XRegAddr.py mit den Registeradressen sowie Modul tofSens.py
+# müssen im selben Verzeichnis sein.
+# Objektorientierte Variante des Skripts, Ausgabe Werte als Floats.
+# S. Mack, 14.11.22
+
+
+import smbus
+import sys
+import math
+import numpy as np
+from time import sleep
+import VL53L1XRegAddr as REG
+from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit
+
+DEBUG_PRINT = False
+
+
+
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+# Variablen ------------------------------------------------------------------
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+calibrated = False;
+
+
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+# main()----------------------------------------------------------------------
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+try:
+    print("Python-Interpreter: {}\n".format(sys.version))
+    i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
+    my_tof = TofSens(i2c, ic2_addr=0x29, timeout_ms=500)
+    if not my_tof.initSensor(True): 
+        print('Failed to detect and initialize sensor!')
+    else:
+        print('Sensor Initialisierung i.O.') 
+    
+    # The minimum timing budget is 20 ms for short distance mode and 33 ms for
+    # medium and long distance modes. See the VL53L1X datasheet for more
+    # information on range and timing limits.
+    my_tof.setDistanceMode(0) # 0=short, 1=medium, 2=long
+    my_tof.setMeasurementTimingBudget(20000) # µs nicht! ms eingeben!
+    # Groesse des ROI festlegen, x=horizontal Laengsrichtung Sensorchip
+    my_tof.setROISize(4,16) # x min 4, y (Hoehe) min 4 max 16
+    while(1):
+        ranges = []
+        # 13 Scanwinkel 0...12 = range(13), 4 Scanwinkel 0,4,8,12 = range(0,13,4)
+        for angle in range(0,13,4):
+            roi_center = 8*angle + 151;
+            my_tof.setROICenter(roi_center) # ROI setzen
+            my_tof.sensorReadSingle(blocking=True) # Abstandswert auslesen (blocking und nur Reichweite)       
+            ranges.append(round(my_tof.ranging_data['range_mm'],0))
+        ranges[0] += 9
+        ranges[1] += 5
+        position_x = [0,0,0,0]
+        position_y = [0,0,0,0]
+        angles = [-10.8, -3.6, 3.6, 10.8]
+        steigung = 0
+        mean_x1 = 0
+        mean_y1 = 0
+        parameter_zaehler = 0
+        parameter_nenner = 0
+        for i in range(0,4):
+        	position_y[i] = round(ranges[i]*math.cos(math.radians(angles[i])),2)
+        	position_x[i] = round(ranges[i]*math.sin(math.radians(angles[i])),2)
+        	mean_x1 += position_x[i]
+        	mean_y1 += position_y[i]
+        	if (i==3):
+        		mean_x = mean_x1/4
+        		mean_y = mean_y1/4	
+        for i in range(0,4):
+        	parameter_zaehler += (position_x[i] - mean_x) * (position_y[i] - mean_y)
+        	parameter_nenner += (position_x[i] - mean_x)**2
+        parameter_b = parameter_zaehler/parameter_nenner
+        steigung = mean_y - parameter_b * mean_x
+        line = np.polyfit(position_x, position_y,1)
+        p = np.poly1d(line)
+        winkel = (180/math.pi)*np.arctan(parameter_b)
+        #winkel_2 = (180/math.pi)*math.atan(p[1])
+        winkel_test = math.atan(p[1])
+        winkel_2 = math.degrees(winkel_test)
+        #print('Range (mm): ',end='')
+        #print(*ranges, sep=', ')
+        #print(parameter_b)
+        #print(ranges)
+        #print(p, 'polyfit')
+        #print(p[1],'polyfit steigung')
+        #print(parameter_b,'steigung berechnet')
+        #print(winkel,'winkel berechnet')
+        print('Abstand',p[0])
+        print('winkel ',winkel_2)
+        #print('x', position_x)
+        #print('y', position_y)
+        #print('range', ranges)
+        #print('Angle (°):  -10.8, -3.6,  3.6,   10.8')
+except KeyboardInterrupt:
+    print(' ')
+    i2c.close()
+    print('...i2c-bus closed.')
+    print('ByeBye')