def imu():
i2c = board.I2C() # uses board.SCL and board.SDA
sensor = LSM6DS33(i2c)
rospy.loginfo("Initializing imu publisher")
imu_pub = rospy.Publisher('/imu', Imu, queue_size=5)
rospy.loginfo("Publishing Imu at: " + imu_pub.resolved_name)
rospy.init_node('imu_node')
rate = rospy.Rate(10) # 50hz
while not rospy.is_shutdown():
rospy.loginfo("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" %
(sensor.acceleration))
rospy.loginfo("Gyro X:%.2f, Y: %.2f, Z: %.2f radians/s" %
(sensor.gyro))
rospy.loginfo("")
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = 'LSM6DS33 6-DoF IMU'
imu.orientation_covariance[0] = -1
imu.angular_velocity = create_vector3(sensor.gyro[0], sensor.gyro[1],
sensor.gyro[2])
imu.linear_acceleration_covariance[0] = -1
imu.linear_acceleration = create_vector3(sensor.acceleration[0],
sensor.acceleration[1],
sensor.acceleration[2])
imu.angular_velocity_covariance[0] = -1
imu_pub.publish(imu)
rate.sleep()
def pedometer():
i2c = board.I2C() # uses board.SCL and board.SDA
sensor = LSM6DS33(i2c)
# enable accelerometer sensor @ 2G and 26 Hz
sensor.accelerometer_range = AccelRange.RANGE_2G
sensor.accelerometer_data_rate = Rate.RATE_26_HZ
# enable the pedometer
sensor.pedometer_enable = True
rospy.loginfo("Initializing pedometer publisher")
ped_pub = rospy.Publisher('/ped', StepCount, queue_size=5)
rospy.loginfo("Publishing StepCount at: " + ped_pub.resolved_name)
rospy.init_node('pedometer_node')
rate = rospy.Rate(1) # 1hz
while not rospy.is_shutdown():
msg = StepCount()
msg.total = sensor.pedometer_steps
ped_pub.publish(msg)
rospy.loginfo(msg)
rate.sleep()
gegg_sprite = displayio.TileGrid(gegg_bmp,
pixel_shader=displayio.ColorConverter())
splash.append(gegg_sprite)
# Draw a label
text_group = displayio.Group(max_size=1, scale=2, x=10, y=220)
text = "Current & Max Acceleration"
text_area = label.Label(terminalio.FONT, text=text, color=0x0000FF)
text_group.append(text_area) # Subgroup for text scaling
splash.append(text_group)
# display everything so far
board.DISPLAY.show(splash)
# connect to the accelerometer
sensor = LSM6DS33(board.I2C())
# highest range for impacts!
sensor.accelerometer_range = AccelRange.RANGE_16G
# we'll read at about 1KHz
sensor.accelerometer_rate = Rate.RATE_1_66K_HZ
# Instead of raw accelerometer data, we'll read the *change* in acceleration (shock)
sensor.high_pass_filter = AccelHPF.SLOPE
sensor.high_pass_filter_enabled = True
# and make a lil buzzer
buzzer = pulseio.PWMOut(board.SPEAKER, variable_frequency=True)
buzzer.frequency = 1000
max_slope = 0
egg_ok = True
while True:
def __init__(self, do_calibrate=True):
"""
Initialize Kamigami interface
"""
# Setup ROS subscribers, publishers
pwm_sub = rospy.Subscriber('kamigami/motor_cmd', PWMCommand,
self.recieve_motor_cmd)
step_sub = rospy.Subscriber('kamigami/step_cmd', StepCommand,
self.recieve_step_cmd)
self.imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=10)
# Setup action clients, servers
# None as of now, should be using one for stepping but VREP only supports services/actions for ROS2
# TODO: Add a debugging mode (multiple levels) or take advantage of ROS's own tools
# Setup IMU
i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = LSM6DS33(i2c)
# TODO: Read in from config file
# Running these at slightly above 100 Hz
# These are currently the default values
self.sensor.accelerometer_data_rate = Rate.RATE_104_HZ
self.sensor.gyro_data_rate = Rate.RATE_104_HZ
self.sensor.accelerometer_range = AccelRange.RANGE_4G
self.sensor.gyro_range = GyroRange.RANGE_250_DPS
# Setup Raspberry Pi Pins
# TODO: Take pin definitions from a config file
self.motor_standby = DigitalOutputDevice(17)
self.motor_right_pwm = PWMOutputDevice(13)
self.motor_left_pwm = PWMOutputDevice(18)
self.motor_right_forward = DigitalOutputDevice(27)
self.motor_right_backward = DigitalOutputDevice(22)
self.motor_left_forward = DigitalOutputDevice(24)
self.motor_left_backward = DigitalOutputDevice(23)
self.pins_on()
# TODO: Take in constants from some kind of config file
self._loop_rate = 100
self._step_pwm = .6
self._step_rest_time = .35
self._left_ang_desired = .09
self._right_ang_desired = -.09
self._threshold = .02
self._filter_alpha = .98
if do_calibrate:
# Calibrate first
print("\nCalibrating! Please keep robot still.")
calibration_time = 3
calibration_start = rospy.get_time()
x_ang_vels = []
x_tilt = []
while (rospy.get_time() - calibration_start) < calibration_time:
x_ang_vels.append(self.sensor.gyro[0])
x_tilt.append(
self.acceleration_to_tilt(self.sensor.acceleration))
# Estimate gyro bias in the roll direction by averaging the angular velocity over all samples
self.x_ang_bias = sum(x_ang_vels) / len(x_ang_vels)
# Estimate current tilt
self.x_tilt_start = sum(x_tilt) / len(x_tilt)
print("\nCalibration finished!")
print(f"Gyro x bias: {self.x_ang_bias}")
print(f"Starting tilt: {self.x_tilt_start}")
# Instance variables
self.roll_estimate = 0
# TODO: Find a better way to represent step commands...
self.queued_left_steps = 3
self.queued_right_steps = 3
# Custom shutdown function to stop all motors
rospy.on_shutdown(self.shutdown)
def parse_fifo_data(data: bytearray) -> None:
"""Parse FIFO data from bytes to decimal."""
return int.from_bytes(data, byteorder='little', signed=True)
# Initialize I2C bus
i2c = busio.I2C(board.SCL, board.SDA, frequency=I2C_FREQUENCY)
# Initialize LSM6DS33
# Resets the device and sets:
# Accelerometer data rate = 104 Hz
# Gyroscope data rate = 104 Hz
# Accelerometer range = 4 g
# Gyroscope range = 250 dps
# BDU = 1 (output registers not updated until MSB and LSB have been read)
imu = LSM6DS33(i2c)
# # Set data rate for accelerometer and gyroscope
# imu.accelerometer_data_rate = Rate.RATE_12_5_HZ
# imu.gyro_data_rate = Rate.RATE_12_5_HZ
# FIFO Config
# Set FIFO watermark to 3072 (0x0C00) words (16 bits/word)
# 3072 samples will take up 6 kbyte, 3/4 of the FIFO
imu._fifo_threshold_l = 0x00
imu._fifo_threshold_h = 0x01
# No decimation for both gyroscope and accelerometer
imu._gyro_fifo_dec = 1
imu._accel_fifo_dec = 1
imu._timer_fifo_dec = 0
# SPDX-FileCopyrightText: Copyright (c) 2020 Bryan Siepert for Adafruit Industries
#
# SPDX-License-Identifier: MIT
import time
import board
import busio
from adafruit_lsm6ds.lsm6ds33 import LSM6DS33
i2c = busio.I2C(board.SCL, board.SDA)
sensor = LSM6DS33(i2c)
while True:
print("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" %
(sensor.acceleration))
print("Gyro X:%.2f, Y: %.2f, Z: %.2f radians/s" % (sensor.gyro))
print("")
time.sleep(0.5)
def Setup_IMU():
i2c = busio.I2C(board.SCL, board.SDA)
s33 = LSM6DS33(i2c)
#print("IMU is set up")
return s33
import time
import subprocess
import board
import busio
from adafruit_lis3mdl import LIS3MDL
from adafruit_lsm6ds.lsm6ds33 import LSM6DS33
i2c = busio.I2C(board.SCL, board.SDA)
senseMag = LIS3MDL(i2c)
senseGyro = LSM6DS33(i2c)
cmd = "hostname -I | cut -d' ' -f1"
IP = subprocess.check_output(cmd, shell=True).decode("utf-8")
while True:
print("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" %
(senseGyro.acceleration))
print("Gyro X:%.2f, Y: %.2f, Z: %.2f radians/s" % (senseGyro.gyro))
print("")
#mag_x, mag_y, mag_z = senseMag.magnetic
print("Magnetic X:{0:10.2f}, Y:{1:10.2f}, Z:{2:10.2f} uT" % (
senseMag.magnetic))
time.sleep(0.5)
fetchDate(words)
if clue:
import math
import digitalio
from adafruit_lsm6ds.lsm6ds33 import LSM6DS33
pin_a = board.BUTTON_A
pin_b = board.BUTTON_B
pin_but_a = digitalio.DigitalInOut(pin_a)
pin_but_a.switch_to_input(pull=digitalio.Pull.UP)
pin_but_b = digitalio.DigitalInOut(pin_b)
pin_but_b.switch_to_input(pull=digitalio.Pull.UP)
left_button = lambda: not pin_but_a.value
right_button = lambda: not pin_but_b.value
keyboard_device = [LSM6DS33(board.I2C()),
right_button,
((0, -2), ### x -z
(1, -2))] ### y -z
default_font = terminalio.FONT
default_font_scale = 2
else:
left_button = right_button = None ### Assume PyPortal
from adafruit_touchscreen import Touchscreen
### These numbers borrowed from
### https://learn.adafruit.com/pyportal-calculator-using-the-displayio-ui-elements
PYPORTAL_TSCAL_X = (5800, 59000)
PYPORTAL_TSCAL_Y = (5800, 57000)
accel = None ### Present but not used
keyboard_device = Touchscreen(board.TOUCH_XL, board.TOUCH_XR,
board.TOUCH_YD, board.TOUCH_YU,
import time
import board
import busio
from adafruit_lsm6ds.lsm6ds33 import LSM6DS33
i2c = busio.I2C(board.SCL, board.SDA)
s33 = LSM6DS33(i2c)
while True:
print("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" % (s33.acceleration))
print("Gyro X:%.2f, Y: %.2f, Z: %.2f radians/s" % (s33.gyro))
print("")
time.sleep(0.5)
def __init__(self):
i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = LSM6DS33(i2c)