diff --git a/VL53L1XtoDistAngle.py b/VL53L1XtoDistAngle.py new file mode 100644 index 0000000000000000000000000000000000000000..011ee8238227873119e79005e306bc7eba68a745 --- /dev/null +++ b/VL53L1XtoDistAngle.py @@ -0,0 +1,110 @@ +#!/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')