def __init__(self):
self.gyro = IMU(dps=250, gyro_bw=800, verbose=False).gyro
self.calibrated = False
self.drift = [0.0,0.0,0.0]
self.xyz = [0.0,0.0,0.0]
self.xyz_dot = [0.0,0.0,0.0]
self.i = 0
def __init__(self, config, queue, level):
self._log = Logger("nxp9dof", level)
if config is None:
raise ValueError("no configuration provided.")
self._config = config
self._queue = queue
_config = self._config['ros'].get('nxp9dof')
self._quaternion_accept = _config.get(
'quaternion_accept') # permit Quaternion once calibrated
self._loop_delay_sec = _config.get('loop_delay_sec')
# verbose will print some start-up info on the IMU sensors
self._imu = IMU(gs=4, dps=2000, verbose=True)
# setting a bias correction for the accel only; the mags/gyros are not getting a bias correction
self._imu.setBias((0.0, 0.0, 0.0), None, None)
# self._imu.setBias((0.1,-0.02,.25), None, None)
_i2c = busio.I2C(board.SCL, board.SDA)
self._fxos = adafruit_fxos8700.FXOS8700(_i2c)
self._log.info('accelerometer and magnetometer ready.')
self._fxas = adafruit_fxas21002c.FXAS21002C(_i2c)
self._log.info('gyroscope ready.')
self._thread = None
self._enabled = False
self._closed = False
self._heading = None
self._pitch = None
self._roll = None
self._is_calibrated = False
self._log.info('ready.')
def ahrs():
#print('')
imu = IMU(verbose=False)
header = 47
#print('-'*header)
#print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r,p,h) [deg]"))
#print('-'*header)
fall = False
#for _ in range(10):
a, m, g = imu.get() # get data from nxp-9dof fxos8700 + fxas21002
r, p, h = imu.getOrientation(
a, m) # convert sensor data to angle in roll,pitch,yaw axis
#print the angle data
#print('| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h))
time.sleep(1)
r = abs(r)
p = abs(p)
#h =abs(h)
if r > 50 or p > 50:
fall = True
else:
fall = False
return fall
def HW_Func():
status = LogicFunctionDisplay([0x70], 1)
psf = LogicFunctionDisplay([0x71, 0x72])
psb = LogicFunctionDisplay([0x73, 0x74, 0x75])
# psb.setBrightness(7) # can be a value between [off] 0-15 [brightest]
imu = IMU() # inerial measurement unit
sub = zmq.Sub(['servos'], ('localhost', 9004))
js_sub = zmq.Sub(['cmd'], ('localhost', 9006))
# you can monitor the publishing of this with:
# topic_echo.py localhost 9005 imu
pub = zmq.Pub(('localhost', 9005))
# create servos
servos = {}
servos['door0'] = Servo(0)
servos['door1'] = Servo(1)
servos['door2'] = Servo(2)
servos['door3'] = Servo(3)
servos['door4'] = Servo(4)
servos['js'] = Servo(7) # this is just for demo
pprint(servos)
while True:
status.update()
psf.update()
psb.update()
time.sleep(0.5)
accel, mag, gyro = imu.get()
msg = Msg.IMU()
msg.linear_acceleration.set(*accel)
# msg.angular_velocity.set(*gyro)
msg.orientation.set(1, 0, 0, 0)
pub.pub('imu', msg)
topic, msg = sub.recv()
if msg:
if topic == 'servos':
print('Topic, Msg:', topic, msg)
angle = msg['angle']
name = msg['name']
servos[name].angle = angle
elif topic == 'cmd':
print('<<< crap cmd in wrong place >>>')
# this is a joystick test
topic, msg = js_sub.recv()
if msg:
if topic == 'cmd':
print('Topic, Msg:', topic, msg)
angle = 90 * msg.angular.x + 90
servos['js'].angle = angle
time.sleep(0.025)
def gyro_test():
imu = IMU(dps=250, gyro_bw=800, verbose=False)
gyro = imu.gyro
while True:
print(gyro.get())
time.sleep(0.1)
def __init__(self):
self.imu = IMU()
self.calibrated = False
self.g_cal = [0.0, 0.0, 0.0]
self._z_accel = None
self.last_a = None
self.last_g = None
self.last_ahrs = None
def __init__(self):
self.gyro = IMU(dps=250, gyro_bw=800, verbose=False).gyro
self.calibrated = False
self.g_cal = [0.0, 0.0, 0.0]
self.xyz = [0.0, 0.0, 0.0]
self.xyz_dot = [None, None, None]
self.calibrate()
self.t0 = None
def background(flag, ns):
# setup arduino
# print("background process started")
print("Starting:", mp.current_process().name)
arduinoSerialData = Arduino(arduino_port, 19200)
imu = IMU(gs=4, dps=2000, verbose=False)
(leds, _, _, _, _) = factory(['leds'])
# print('flag', flag.is_set())
while flag.is_set():
a, m, g = imu.get()
ns.accels = a # [x,y,z]
ns.mags = m
ns.gyros = g
a = normalize(a)
# seems that 0.80 is pretty big tilt
if a[2] < 0.85:
ns.safety_kill = True
print(a)
print('<<< TILT >>>')
# read battery
arduinoSerialData.write('2')
d = arduinoSerialData.readline()
if d:
batt = float(d)
ns.battery = batt
# read ultrasound
arduinoSerialData.write('1')
for u in [ns.usound0, ns.usound1, ns.usound2, ns.usound3]:
d = arduinoSerialData.readline()
if d:
u = float(d)
# update LEDs
fpsi = ns.logicdisplay['fpsi']
if fpsi == 0:
leds[0].setSolid(1)
elif fpsi == 1:
leds[0].setSolid(3)
elif fpsi == 2:
leds[0].setSolid(2)
for led in leds[1:]:
led.setRandom()
time.sleep(1)
# clean up
for led in leds:
led.clear()
def __init__(self, data):
# mp.Process.__init__(self)
self.robot = Engine(data)
netural = self.robot.getFoot0(0)
self.gait = {'crawl': DiscreteRippleGait(45.0, netural)}
if platform.system() == 'Linux':
self.imu = IMU()
else:
self.imu = None
self.js = Joystick()
def __init__(self, md_pins):
# mp.Process.__init__(self)
# logging.basicConfig(level=logging.INFO)
# self.logger = logging.getLogger('robot')
if len(md_pins) != 8:
raise Exception('Wrong number of pins for motor driver!')
self.md = md.MotorDriver(*md_pins)
self.digital_ir = DigitalIR([1, 2, 3, 4])
self.analog_ir = AnalogIR()
self.imu = IMU()
self.ahrs = AHRS(True)
def imu():
imu = IMU(gs=4, dps=2000, verbose=True)
header = 67
print('-' * header)
print("| {:17} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]",
"Gyros [dps]"))
print('-' * header)
accRaw = np.empty([1, 3])
magRaw = np.empty([1, 3])
RadToDeg = 360 / 2 * math.pi
for _ in range(1000):
a, m, g = imu.get()
print(
'| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'
.format(a[0], a[1], a[2], m[0], m[1], m[2], g[0], g[1], g[2]))
magRaw[0] = m[0]
magRaw[1] = m[1]
magRaw[2] = m[2]
accRaw[0] = a[0]
accRaw[1] = a[1]
accRaw[2] = a[2]
headingraw = math.atan2(magRaw[1], magRaw[0]) * RadToDeg
xM = magRaw[0]
yM = magRaw[1]
zM = magRaw[2]
xG = accRaw[0]
yG = accRaw[1]
zG = accRaw[2]
pitch = math.atan2(xG, math.sqrt(yG * yG + zG * zG))
roll = math.atan2(yG, zG)
roll_print = roll * RadToDeg
pitch_print = pitch * RadToDeg
xM2 = xM * math.cos(pitch) + zM * math.sin(pitch)
yM2 = xM * math.sin(roll) * math.sin(pitch) + yM * math.cos(
roll) - zM * math.sin(roll) * math.cos(pitch)
compheading = math.atan2(yM2, xM2) * RadToDeg
compheading = compheading + 90
if compheading <= 0:
compheading = compheading * -1
elif compheading > 0:
compheading = 360 - compheading
print("compheading: ", compheading)
time.sleep(1)
def ahrs():
imu = IMU()
ahrs = AHRS(True)
print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r,p,h) [deg]"))
print('-' * 47)
for _ in range(10):
a, m, g = imu.get()
r, p, h = ahrs.getOrientation(a, m)
print('| {:>6.2f} {:>6.2f} {:>6.2f} | {:>6.2f} {:>6.2f} {:>6.2f} |'.
format(a[0], a[1], a[2], r, p, h))
time.sleep(1.0)
def imu():
imu = IMU()
print("| {:20} | {:20} | {:20} |".format("Accels [g's]", " Magnet []",
"Gyros [dps]"))
print('-' * 47)
for _ in range(10):
a, m, g = imu.get()
# r, p, h = ahrs.getOrientation(a, m)
print('| {:>6.2f} {:>6.2f} {:>6.2f} | {:>6.2f} {:>6.2f} {:>6.2f} |'.
format(a[0], a[1], a[2], m[0], m[1], m[2]))
time.sleep(1.0)
def check_cal():
bag = Bag('check_cal.dat', ['imu'])
imu = IMU()
for i in range(1000):
# grab IMU
if i % 100 == 0:
print('step:', i)
a, m, g = imu.get()
bag.push('imu', (a, m, g))
time.sleep(0.03)
bag.close()
print('done ...')
def main():
port = '/dev/ttyUSB0'
bot = bot = pycreate2.Create2(port)
bot.start()
bot.safe()
bot.drive_stop()
image_size = (320, 240)
camera = Camera(cam='pi')
camera.init(win=image_size)
imu = IMU()
bag = BagWriter()
bag.open(['camera', 'accel', 'mag', 'cr'])
bag.stringify('camera')
bag.use_compression = True
try:
for i in range(100):
ret, frame = camera.read()
if ret:
bag.push('camera', frame)
else:
print('>> bad frame', i)
sen = bot.get_sensors()
bag.push('cr', sen)
a, m, g = imu.get()
bag.push('accel', a)
bag.push('mag', m)
# time.sleep(0.025)
print(i)
except KeyboardInterrupt:
print(' ctrl-c pressed, bye ...')
except Exception as e:
print('')
print('Something else happened ... :(')
print('Maybe the robot is not on ... press the start button')
print('-'*30)
print(e)
bag.write('test.json')
def imu():
imu = IMU(gs=4, dps=2000, verbose=True)
header = 67
print('-' * header)
print("| {:17} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]",
"Gyros [dps]"))
print('-' * header)
for _ in range(10):
a, m, g = imu.get()
print(
'| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'
.format(a[0], a[1], a[2], m[0], m[1], m[2], g[0], g[1], g[2]))
time.sleep(0.50)
print('-' * header)
print(' uT: micro Tesla')
print(' g: gravity')
print('dps: degrees per second')
print('')
def ahrs():
print('')
imu = IMU(verbose=True)
header = 47
print('-'*header)
print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r,p,h) [deg]"))
print('-'*header)
for _ in range(10):
a, m, g = imu.get()
r, p, h = imu.getOrientation(a, m)
print('| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h))
time.sleep(0.50)
print('-'*header)
print(' r: roll')
print(' p: pitch')
print(' h: heading')
print(' g: gravity')
print('deg: degree')
print('')
def __init__(self, port='/dev/ttyUSB0'):
self.bot = pycreate2.Create2(port)
# only create if on linux:
image_size = (640, 480)
if platform.system() == 'Linux':
self.camera = Camera(cam='pi')
self.camera.init(win=image_size)
self.imu = IMU()
else:
self.camera = Camera(cam='cv')
self.camera.init(win=image_size, cameraNumber=0)
self.sensors = {'create': None, 'imu': None, 'camera': None}
self.distance = 0.0
self.ahrs = AHRS()
self.modes = {
'js': JoyStickMode(self.bot),
'demo': DemoMode(self.bot),
'auto': AutoMode(self.bot),
'idle': IdleMode(self.bot),
'sensors': Sensors(self.bot)
}
# self.bag = Bag('./data/robot-{}.json'.format(time.ctime().replace(' ', '-')), ['imu', 'camera', 'create'])
self.current_mode = 'idle'
self.last_time = time.time()
self.data = {
# 'r': CircularBuffer(30),
# 'p': CircularBuffer(30),
'y': CircularBuffer(30),
}
def write():
bag = Bag(filename, ['imu', 'create'])
# cap = cv2.VideoCapture(0)
imu = IMU()
bot = pycreate2.Create2('/dev/ttyUSB0')
bot.start()
bot.safe()
for i in range(100):
# grab IMU
a, m, g = imu.get()
bag.push('imu', (a, m, g))
# grab create sensors
s = bot.inputCommands()
bag.push('create', s)
# grab images
# ret, frame = cap.read()
# if not ret:
# frame = None
time.sleep(0.03)
from squaternion import euler2quat
# from squaternion import Quaternion
import os
if 1:
from fake_rpi import toggle_print
toggle_print(False)
if __name__ == "__main__":
if 'TRAVIS' in os.environ:
print(">> FOUND TRAVIS\n\n")
geckopy.init_node()
pub = Pub()
pub.bind(zmqUDS('/tmp/uds-lidar'))
rate = geckopy.Rate(20)
imu = IMU(gs=2, dps=2000, verbose=False)
imu.setBias((0.1, -0.02, .25), None, None)
msg = Imu()
while not geckopy.is_shutdown():
a, m, g = imu.get()
msg.linear_acceleration.x = a[0]
msg.linear_acceleration.y = a[1]
msg.linear_acceleration.z = a[2]
msg.angular_velocity.x = g[0]
msg.angular_velocity.y = g[1]
msg.angular_velocity.z = g[2]
#!/usr/bin/env python3
from nxp_imu import IMU
import time
from the_collector import BagIt
from the_collector import Pickle
from collections import namedtuple
if __name__ == "__main__":
imu = IMU(gs=4, dps=2000, verbose=True)
bag = BagIt(Pickle)
Data = namedtuple('Data', 'a m g timestamp')
for _ in range(1000):
a, m, g = imu.get()
ts = time.time()
# d = Data(a, m, g, time.time())
bag.push('accel', (
a,
ts,
))
bag.push('mag', (
m,
ts,
))
bag.push('gyro', (
g,
ts,
))
from nxp_imu import IMU
# from nxp_imu import AHRS
import time
import simplejson as json
# def print_data(data):
# print("| {:20} | {:20} |".format("Accels [g's]", "Orient(r, p, h) [deg]"))
# print('-'*47)
# for a in data['accel']:
# # a, m, g = imu.get()
# # r, p, h = ahrs.getOrientation(a, m)
# print('| {:>6.2f} {:>6.2f} {:>6.2f} | {:>6.2f} {:>6.2f} {:>6.2f} |'.format(a[0], a[1], a[2], r, p, h))
if __name__ == "__main__":
try:
imu = IMU()
# ahrs = AHRS(True)
data = {
'accel': [],
'mag': [],
'gyro': [],
}
while True:
a, m, g = imu.get()
data['accel'].append(a)
data['mag'].append(m)
data['gyro'].append(g)
time.sleep(0.1)
except KeyboardInterrupt:
def __init__(self):
self.imu = IMU() # inerial measurement unit
self.currentSensor = None # placeholder
self.adc = None
from imutils.video import VideoStream
from nxp_imu import IMU
from pydar import URG04LX
import platform
from collections import namedtuple
import time
import pickle
Data_ts = namedtuple('Data_ts', 'data timestamp')
data = {'imu': [], 'camera': [], 'lidar': []}
if __name__ == "__main__":
print("<<< START >>>")
imu = IMU(gs=2, dps=250)
if platform.system().lower() == 'linux':
args = {
'src': 0,
'usePiCamera': True,
'resolution': (
640,
480,
),
'framerate': 10
}
port = '/dev/serial/by-id/usb-Hokuyo_Data_Flex_for_USB_URG-Series_USB_Driver-if00'
else:
args = {'src': 0, 'usePiCamera': False}
def accel_test():
imu = IMU(dps=250, gyro_bw=800, verbose=False)
def i2c_proc(flag, ns):
"""
Everything attached to i2c bus goes here so we don't have to do semifores.
Also, the lcd button code is here too so it is always in sync with the
led matricies.
"""
print("Starting:", mp.current_process().name)
imu = IMU(gs=4, dps=2000, verbose=False)
leds = LogicFunctionDisplay(led_data)
led_update = 0
servos = []
servo_angles = []
for id in servo_limits:
s = Servo(id)
# s.setMinMax(*servo_limits[id]) # set open(min)/closed(max) angles
s.open = servo_limits[id][0]
s.close = servo_limits[id][1]
s.closeDoor() # closed
# servo_angles.append(sum(servo_limits[id])/2)
servos.append(s)
servo_angles.append(s.angle)
time.sleep(0.01)
# init namespace to have the same angles
ns.servo_angles = servo_angles
Servo.all_stop()
b_led = ButtonLED(26, 16, 20)
# test ---------------------
# vals = [True]*3
# b_led.setRGB(*vals)
# time.sleep(3)
# for i in range(3):
# print(i)
# vals = [True]*3
# vals[i] = False
# b_led.setRGB(*vals)
# time.sleep(3)
while flag.is_set():
a, m, g = imu.get()
ns.accels = a # accel [x,y,z] g's
ns.mags = m # magnetometer [x,y,z] mT
ns.gyros = g # gyros [x,y,z] rads/sec
# FIXME: real hw is at a funny oriendataion
# a = normalize(a)
# seems that 0.80 is pretty big tilt
# if a[2] < 0.85:
# ns.safety_kill = True
# print(a)
# print('<<< TILT >>>')
# update LEDs
# OFF = 0
# GREEN = 1
# RED = 2
# YELLOW = 3
led_update += 1
if led_update % 20 == 0:
led_update = 0
# print('current_state',ns.current_state)
cs, batt = ns.current_state, ns.battery
if cs == 1: # standby
csc = 2 # red
b_led.setRGB(True, False, False)
elif cs == 2: # static
csc = 3 # yellow
b_led.setRGB(True, True, True)
elif cs == 3: # remote
csc = 1 # green
b_led.setRGB(False, True, False)
# make something up for now
# battc = random.randint(1,3)
if ns.set_all_leds:
leds.setAll(ns.set_all_leds)
else:
leds.setFLD(csc, 1)
leds.setRLD()
# update servos if the have changed
# namespace.servo_angles: another process wants to change the angle
# servo_angles: local copy, if no difference between the 2, do nothing
# TODO: should these just be open/close (T/F)? why angles?
for nsa, sa, servo in zip(ns.servo_angles, servo_angles, servos):
if nsa == sa:
continue
sa = nsa
servo.angle = sa
time.sleep(0.1)
if ns.servo_wave:
ns.servo_wave = False
leds.setAll(1)
print('servo wave')
for s in servos:
s.openDoor()
time.sleep(0.2)
# servos[1].stop()
time.sleep(2)
# Servo.all_stop()
for s in servos:
s.closeDoor()
time.sleep(0.2)
# servos[1].stop()
time.sleep(2)
Servo.all_stop()
b_led.setRGB(False, False, False)
def read9():
#--------9dof init----------
imu = IMU(gs=2, dps=2000, verbose=False)
i2c = I2C(0x1F)
#--------Set calibration values for magnetometer---------
'''Splits avarages to MSB and LSB'''
XMSB = (Xavg / 256) & 0xFF
XLSB = Xavg & 0xFF
YMSB = (Yavg / 256) & 0xFF
YLSB = Yavg & 0xFF
ZMSB = (Zavg / 256) & 0xFF
ZLSB = Zavg & 0xFF
'''Writes splitted avarages to offset registers'''
i2c.write8(0x3F, int(XMSB))
i2c.write8(0x40, int(XLSB))
i2c.write8(0x41, int(YMSB))
i2c.write8(0x42, int(YLSB))
i2c.write8(0x43, int(ZMSB))
i2c.write8(0x44, int(ZLSB))
#--------Talker node init------------
#pub = rospy.Publisher('chatter', String, queue_size=10)
pub = rospy.Publisher('imu', Imu)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 25hz
#---------Readloop---------
while not rospy.is_shutdown():
Havg = 0
#Collects samples and returns avaraged value.
for x in range(0, 20):
try:
a, m, g = imu.get() #Gets data from sensors
r, p, h = imu.getOrientation(
a, m) #calculates orientation from data
ac, ma = imu.rawOrientation() #raw data for magnetometer
'''
#Prevents error in Havg when heading fluctuates between 359 and 0.
if(x > 1):
if(Havg > 260):
if(h < 100):
h += 360
if(Havg < 100):
if(h > 260):
h -= 360
Havg = (Havg + h)/2
'''
except IOError:
pass
'''
#Corrects the heading so that 0 < Havg < 359.
if(Havg < 0):
Havg += 360
if(Havg >= 360):
Havg -= 360
heading = str(Havg)
'''
roll = r * (3.141592 / 180.0) #converts degrees to radians
pitch = p * (3.141592 / 180.0)
yaw = h * (3.141592 / 180.0)
'''
a[0] = a[0] * 9.806 #converts g's to m/s^2
a[1] = a[1] * 9.806
a[2] = a[2] * 9.806
g[0] = g[0] * (3.141592/180.0) #converts degrees to radians
g[1] = g[1] * (3.141592/180.0)
g[2] = g[2] * (3.141592/180.0)
'''
print("Heading: " + str(h))
#q = toQuaternion(r, p, h)
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0] #magnetometer
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.angular_velocity.x = g[0] * (3.141592 / 180.0) #gyro
imuMsg.angular_velocity.y = g[1] * (3.141592 / 180.0)
imuMsg.angular_velocity.z = g[2] * (3.141592 / 180.0)
imuMsg.linear_acceleration.x = a[0] * 9.806 #accelerometer
imuMsg.linear_acceleration.y = a[1] * 9.806
imuMsg.linear_acceleration.z = a[2] * 9.806
rospy.loginfo(imuMsg)
pub.publish(imuMsg)
rate.sleep()