Initial Commit

README.md

+# Projekt Automatisierungstechnik
+
+## "Drehbuch"
+
+  - [ ] vor 29.3.  
+    - [x] ROS kennenlernen  
+    - [ ] ROS Skripte vorbereiten  
+  - [ ] am 29.3.  
+    - [ ] Betrieb mit WLAN  
+    - [ ] ROS Knoten realisieren  
+  - [ ] vor/am 5.4.  
+    - [ ] TOF Sensor auslesen, auswerten, Winkel ausgeben  
+  - [ ] vor/am 12.4.  
+    - [ ] Simulink zu ROS verstehen  
+    - [ ] Kalmanfilter in Simulink umsetzen
+
+## ROS
+Die Python Skripte für die ROS-Knoten werden im catkin_ws (workspace) unter `catkin_ws/src/scripts/` abgelegt,
+so können sie einfach mit `rosrun pat_ros my_node` gestartet werden.
+
+
+Um nicht jeden Knoten einzeln starten zu müssen wird `roslaunch` verwendet.
+Es nutzt eine Art config-Datei unter `catkin_ws/src/launch/` in der beschrieben ist welcher Knoten wo und wie gestartet werden soll.
+`roslaunch` führt für uns `roscore` und `rosrun` aus.
+
+Der Aufbau des ROS-Netzwerks kann mit `rqt_graph` am PC angezeigt werden.
+
+Alle Topics, also Nachrichten-Arten `rostopic list` kann de
\ No newline at end of file

TOFx4.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 rospy
+from std_msgs.msg import UInt16MultiArray
+import smbus
+import sys
+from time import sleep
+import VL53L1XRegAddr as REG
+from tofSens import TofSens, countRateFixedToFloat, writeReg, writeReg16Bit, writeReg32Bit, readReg, readReg16Bit
+
+def talker():
+    pub = rospy.Publisher('range_val', UInt16MultiArray, queue_size=10)
+    rospy.init_node('TOFx4', anonymous=False)
+    rate = rospy.Rate(10) # maximum rate 10 Hz due to sensor response time
+    
+    try:
+        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 not rospy.is_shutdown():
+            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))
+            print('Range (mm): ',end='')
+            print(*ranges, sep=', ')
+            
+            pub.publish(UInt16MultiArray(data=ranges))
+            rate.sleep()
+    
+    except KeyboardInterrupt:
+        print('KeyboardInterrupt')
+
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+# main()----------------------------------------------------------------------
+# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+if __name__ == '__main__':
+    try:
+        print('TOFx4 startet...')
+        i2c = smbus.SMBus(2) # I2C-Kommunikation aktivieren
+        talker()
+    except rospy.ROSInterruptException:
+        print('ROSInterruptException')
+        pass
+    finally:
+        i2c.close()
+        print('...i2c-bus closed.')
+        print('Byebye...')
\ No newline at end of file