def runExample():
print("\nSparkFun 9DoF ICM-20948 Sensor Example 1\n")
IMU = qwiic_icm20948.QwiicIcm20948()
if IMU.connected == False:
print("The Qwiic ICM20948 device isn't connected to the system. Please check your connection", \
file=sys.stderr)
return
IMU.begin()
while True:
if IMU.dataReady():
IMU.getAgmt(
) # read all axis and temp from sensor, note this also updates all instance variables
print(\
'{: 06d}'.format(IMU.axRaw)\
, '\t', '{: 06d}'.format(IMU.ayRaw)\
, '\t', '{: 06d}'.format(IMU.azRaw)\
, '\t', '{: 06d}'.format(IMU.gxRaw)\
, '\t', '{: 06d}'.format(IMU.gyRaw)\
, '\t', '{: 06d}'.format(IMU.gzRaw)\
, '\t', '{: 06d}'.format(IMU.mxRaw)\
, '\t', '{: 06d}'.format(IMU.myRaw)\
, '\t', '{: 06d}'.format(IMU.mzRaw)\
)
time.sleep(0.03)
else:
print("Waiting for data")
time.sleep(0.5)
def imu_publisher():
# start the IMU node
rospy.init_node('imu')
# create a publisher, especially with the IMU we have no use for old data
# so we set the buffer size to 1
pub = rospy.Publisher('imupub', IMU, queue_size=1)
# establish the data rate according to the user parameter set above
rate = rospy.Rate(imu_odr)
# create an object of the type
IMUchip = qwiic_icm20948.QwiicIcm20948()
# check that the constructor worked as expected, exit if not
if IMUchip.connected == False:
print(
"The Qwiic ICM20948 device isn't connected to the system. Please check your connection",
file=sys.stderr)
return
# start up the chip now that we know we can talk to it
IMUchip.begin()
# create an empty message
message = IMU()
while not rospy.is_shutdown():
# make sure it is ready for use before banging registers
if IMUchip.dataReady():
# built in function which gets it all for us
IMUchip.getAgmt()
# always fill in the time stamp in the header
message.accelerometer.header.stamp = rospy.Time.now()
message.gyroscope.header.stamp = message.accelerometer.header.stamp
message.magnetometer.header.stamp = message.accelerometer.header.stamp
# Package the imu data into IMU message format and publish
message.gyroscope.x = IMUchip.gxRaw
message.gyroscope.y = IMUchip.gyRaw
message.gyroscope.z = IMUchip.gzRaw
message.accelerometer.x = IMUchip.axRaw
message.accelerometer.y = IMUchip.ayRaw
message.accelerometer.z = IMUchip.azRaw
message.magnetometer.x = IMUchip.mxRaw
message.magnetometer.y = IMUchip.myRaw
message.magnetometer.z = IMUchip.mzRaw
# publish the message
pub.publish(message)
# wait for a calculated amount of time before doing it all over again
rate.sleep()
def runExample():
print("\nSparkFun SerLCD and 9DoF ICM-20948 Sensor Example\n")
myLCD = qwiic_serlcd.QwiicSerlcd()
IMU = qwiic_icm20948.QwiicIcm20948()
if myLCD.connected == False:
print("The Qwiic SerLCD device isn't connected to the system. Please check your connection", \
file=sys.stderr)
return
if IMU.connected == False:
print("The Qwiic ICM20948 device isn't connected to the system. Please check your connection", \
file=sys.stderr)
return
myLCD.setBacklight(255, 255, 255) # Set backlight to bright white
myLCD.setContrast(5) # Set contrast. Lower to 0 for higher contrast.
myLCD.clearScreen(
) # Clear Screen - this moves the cursor to the home position as well
myLCD.print("white") # Write to color name to SerLCD
time.sleep(0.5) # give a sec for system messages to complete
IMU.begin()
while True:
#declare ledColor a global variable inside here to access it
global ledColor
if IMU.dataReady():
IMU.getAgmt(
) # read all axis and temp from sensor, note this also updates all instance variables
#the following are the threshold values for each axis is pointing right-side up
# anything above IMU.azRaw > 16000 is red
# ledColor = 1
aZPos = 16000
# anything below IMU.azRaw < -16000 is blue
# ledColor = 2
aZNeg = -16000
# anything above IMU.ayRaw > 16100 is green
# ledColor = 3
ayPos = 16100
# anything below IMU.ayRaw < -16000 is green
# ledColor = 4
ayNeg = -16000
# anything above IMU.axRaw > 16000 is magenta
# ledColor = 5
axPos = 16000
# anything below IMU.axRaw < -16400 is cyan
# ledColor = 6
axNeg = -16400
#adjust color of the LED based on the accelerometer's reading
if IMU.azRaw > aZPos:
# Set LED red
myLCD.setBacklight(255, 0, 0) # Set backlight to bright white
ledColor = 1
myLCD.clearScreen()
myLCD.print("red")
elif IMU.azRaw < aZNeg:
# Set LED blue
myLCD.setBacklight(0, 0, 255) # Set backlight to bright white
ledColor = 2
myLCD.clearScreen()
myLCD.print("blue")
elif IMU.ayRaw > ayPos:
# Set LED yellow
myLCD.setBacklight(255, 255,
0) # Set backlight to bright white
ledColor = 3
myLCD.clearScreen()
myLCD.print("yellow")
elif IMU.ayRaw < ayNeg:
# Set LED green
myLCD.setBacklight(0, 255, 0) # Set backlight to bright white
ledColor = 4
myLCD.clearScreen()
myLCD.print("green")
elif IMU.axRaw > axPos:
# Set LED magenta
myLCD.setBacklight(255, 0,
255) # Set backlight to bright white
ledColor = 5
myLCD.clearScreen()
myLCD.print("magenta")
elif IMU.axRaw < axNeg:
# Set LED cyan
myLCD.setBacklight(0, 255,
255) # Set backlight to bright white
ledColor = 6
myLCD.clearScreen()
myLCD.print("cyan")
if ledColor == 1:
print("ledColor = red", '\n', '\n')
elif ledColor == 2:
print("ledColor = blue", '\n', '\n')
elif ledColor == 3:
print("ledColor = yellow", '\n', '\n')
elif ledColor == 4:
print("ledColor = green", '\n', '\n')
elif ledColor == 5:
print("ledColor = magenta", '\n', '\n')
elif ledColor == 6:
print("ledColor = cyan", '\n', '\n')
aX = IMU.axRaw
aY = IMU.ayRaw
aZ = IMU.azRaw
gX = IMU.gxRaw
gY = IMU.gyRaw
gZ = IMU.gzRaw
mX = IMU.mxRaw
mY = IMU.myRaw
mZ = IMU.mzRaw
# Remove the `#` for the following lines to
# display accelerometer readings on SerLCD
#myLCD.setCursor(8,0)
#myLCD.print("aX")
#myLCD.print(str(aX))
#myLCD.setCursor(0,1)
#myLCD.print("aY")
#myLCD.print(str(aY))
#myLCD.setCursor(8,1)
#myLCD.print("aZ")
#myLCD.print(str(aZ))
print(\
' aX:', '{: 4.1f}'.format(aX)\
, ' \t, aY:', '{: 4.1f}'.format(aY)\
, '\t, aZ:', '{: 4.1f}'.format(aZ)\
, '\n gX:', '{: 4.1f}'.format(gX)\
, '\t, gY:', '{: 4.1f}'.format(gY)\
, '\t, gZ:', '{: 4.1f}'.format(gZ)\
, '\n mX:', '{: 4.1f}'.format(mX)\
, '\t, mY:', '{: 4.1f}'.format(mY)\
, '\t, mZ:', '{: 4.1f}'.format(mZ)\
, '\n'\
)
time.sleep(1) # small delay so that the screen doesn't flicker
else:
print("Waiting for data")
time.sleep(0.5)
"--"]]) # initialize an array for storing AT data
WTdata = np.array(
[["Temp(C)", "Year", "Month", "Day", "Hour", "Min", "Sec", "Lat", "Long"],
["--", "--", "--", "--", "--", "--", "--", "--",
"--"]]) # initialize an array for storing WT data
# initialize an array for storing IMU data
IMUdata = np.array([[
"Date", "Time", "AccelX", "AccelY", "AccelZ", "GyroX", "GyroY", "GyroZ",
"MagX", "MagY", "MagZ", "Temp"
], ["--", "--", "--", "--", "--", "--", "--", "--", "--", "--", "--", "--"]])
# Initialize the 8-channel Qwiic Multiplexer
mux = qwiic.QwiicTCA9548A()
# Create an IMU object
IMU = qwiic_icm20948.QwiicIcm20948()
mux.disable_channels([0, 1, 2, 3, 4, 5, 6, 7])
mux.enable_channels([0])
IMU.begin()
# Main loop runs forever printing the location, etc. every second.
last_log = time.monotonic()
imu_log = time.monotonic()
h2o_time = time.monotonic()
air_time = time.monotonic()
imu_cycle = time.monotonic()
start_time = time.monotonic()
while infLoop:
# Checks for new GPS data
gps.update()
mcp6_p5 = AnalogIn(mcp6, MCP.P5)
mcp6_p6 = AnalogIn(mcp6, MCP.P6)
mcp6_p7 = AnalogIn(mcp6, MCP.P7)
# MCP connected to D13 #
mcp13_p0 = AnalogIn(mcp13, MCP.P0)
mcp13_p1 = AnalogIn(mcp13, MCP.P1)
mcp13_p2 = AnalogIn(mcp13, MCP.P2)
mcp13_p3 = AnalogIn(mcp13, MCP.P3)
mcp13_p4 = AnalogIn(mcp13, MCP.P4)
mcp13_p5 = AnalogIn(mcp13, MCP.P5)
mcp13_p6 = AnalogIn(mcp13, MCP.P6)
mcp13_p7 = AnalogIn(mcp13, MCP.P7)
# INITIALIZE IMU(s) #
IMU_1 = qwiic_icm20948.QwiicIcm20948()
if IMU_1.connected == False:
print("The Qwiic ICM20948 device isn't connected to the system. Please check your connection", \
file=sys.stderr)
IMU_1.begin()
#TODO figure out how to read from both IMUs. look into the setup py from the IMU library
# print training data to JSON file #
sensor_data = {}
sensor_data['SIGN'] = []
sensor_data['MCP5'] = []
sensor_data['MCP6'] = []
sensor_data['MCP13'] = []