def imu_publisher(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(10)
p = geckopy.Publisher()
test = kwargs.get('test', False)
imu = IMU_hw()
while not geckopy.is_shutdown():
if test: # fake readings
msg = IMU(
Vector(1, 2, 3),
Vector(1, 2, 3),
Vector(1, 2, 3),
)
else:
a, m, g = imu.get()
geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a))
msg = IMU(Vector(*a), Vector(*m), Vector(*g))
p.pub('imu', msg)
msg = Vector(0, 0, 1)
p.pub('unit_accel', msg)
# sleep
rate.sleep()
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
class GYRO_FXAS21002:
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 get(self):
self.i += 1
x_dot, y_dot, z_dot = self.gyro.get()
return np.subtract((x_dot, y_dot, z_dot), self.drift), time.time()
def start(self):
self.calibrate()
print('sample reading from GYRO_FXAS21002', self.gyro.get())
def calibrate(self):
print('Calibrating GYRO_FXAS21002...')
for i in range(10):
self.gyro.get()
for i in range(400):
x, y, z = self.gyro.get()
v = (x, y, z)
self.drift = np.add(v, self.drift)
self.drift = np.divide(self.drift, 400)
print('Gyro calibration:', self.drift)
def __str__(self):
return 'GYRO_FXAS21002 i=' + str(self.i)
def imu_publisher(**kwargs):
# geckopy.init_node(**kwargs)
rate = geckopy.Rate(20)
# p = geckopy.Publisher()
# test = kwargs.get('test', False)
imu = IMU_hw()
save = []
# while not geckopy.is_shutdown():
for i in range(400):
a, m, g = imu.get()
# geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a))
print(">> {}".format(i % 20))
msg = IMU(Vector(*a), Vector(*m), Vector(*g))
save.append(msg)
# p.pub('imu', msg)
# msg = Vector(0,0,1)
# p.pub('unit_accel', msg)
# sleep
rate.sleep()
pickle.dump(save, open("accel-z-up.pickle", "wb"))
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 imu_publisher(**kwargs):
geckopy.init_node(**kwargs)
rate = geckopy.Rate(10)
p = geckopy.Publisher()
if platform.system() == 'Linux':
isLinux = True
imu = IMU_hw()
else:
isLinux = False
imu = None
while not geckopy.is_shutdown():
if isLinux:
a, m, g = imu.get()
# geckopy.log('{:.1f} {:.1f} {:.1f}'.format(*a))
msg = IMU(Vector(*a), Vector(*m), Vector(*g))
else: # fake readings
msg = IMU(
Vector(1, 2, 3),
Vector(1, 2, 3),
Vector(1, 2, 3),
)
p.pub('imu', msg)
# msg = Vector(0,0,1)
# p.pub('unit_accel', msg)
# sleep
rate.sleep()
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 __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 __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 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 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 __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 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)
class GYRO_FXAS21002:
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 get_angular_velocity(self):
x_dot, y_dot, z_dot = self.gyro.get()
t1 = time.time()
self.xyz_dot = np.subtract((x_dot, y_dot, z_dot), self.g_cal)
if self.t0 == None:
self.t0 = time.time()
return [0.0, 0.0, 0.0], 0.0
else:
dt = t1 - self.t0
self.t0 = t1
return self.xyz_dot, dt
def get(self):
x_dot, y_dot, z_dot = self.gyro.get()
t1 = time.time()
self.xyz_dot = np.subtract((x_dot, y_dot, z_dot), self.g_cal)
if self.t0 == None:
self.t0 = time.time()
return [0.0, 0.0, 0.0], 0.0
else:
self.xyz = np.add(self.xyz, self.xyz_dot)
dt = t1 - self.t0
self.t0 = t1
return self.xyz, dt
def calibrate(self):
print('Calibrating GYRO_FXAS21002...')
for i in range(10):
self.gyro.get()
for i in range(400):
x, y, z = self.gyro.get()
v = (x, y, z)
self.g_cal = np.add(v, self.g_cal)
self.g_cal = np.divide(self.g_cal, 400)
print('Gyro calibration:', self.g_cal)
def __str__(self):
return 'gyro' + str(self.xyz)
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_pub():
geckopy.init_node()
rate = geckopy.Rate(2)
p = geckopy.pubBinderTCP(KEY, "imu")
if p is None:
raise Exception("publisher is None")
imu = NXP_IMU()
while not geckopy.is_shutdown():
msg = imu.get()
p.publish(msg) # topic msg
rate.sleep()
print('imu pub bye ...')
def gyro_test():
imu = IMU(dps=250, gyro_bw=800, verbose=False)
gyro = imu.gyro
while True:
print(gyro.get())
time.sleep(0.1)
class Sensors(object):
def __init__(self):
self.imu = IMU() # inerial measurement unit
self.currentSensor = None # placeholder
self.adc = None
def getSensors(self):
accel, mag, gyro = self.imu.get()
return (accel, mag, gyro)
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, 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, 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)
class SoccerRobot(object):
"""
This is the low level robot hardware driver:
- motor driver x2
- ADC (8 ch): IR
"""
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 __del__(self):
self.md.allStop()
def shutdown(self):
"""
Needed?
"""
pass
def makeCmd(self, msg):
# msg should be a twist FIXME
m0 = m1 = m2 = m3 = (0, 0)
return m0, m1, m2, m3
def loop_once(self):
while True:
# get sensors
self.analog_ir.read()
ret = self.imu.get()
self.accel = ret[:3]
self.mag = ret[3:6]
self.gyro = ret[6:]
self.orientation = self.ahrs.getOrientation(self.accel, self.mag)
time.sleep(0.05) # 0.05 => 20Hz
class SimpleQuadruped(object):
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 run(self):
while True:
if self.js.valid:
ps4 = self.js.get()
x, y = ps4['leftStick']
rz, _ = ps4['rightStick']
cmd = (x, y, rz)
else:
cmd = (1, 0, 0)
# read ahrs
if self.imu:
a, m, g = self.imu.read()
# roll, pitch, heading = d
# if (-90.0 > roll > 90.0) or (-90.0 > pitch > 90.0):
# print('Crap we flipped!!!')
# cmd = (0, 0, 0)
print('imu', a, m, g)
print('***********************************')
# print('* rest {:.2f} {:.2f} {:.2f}'.format(*leg))
# print('ahrs[deg]: roll {:.2f} pitch: {:.2f} yaw: {:.2f}'.format(d[0], d[1], d[2]))
print('* cmd {:.2f} {:.2f} {:.2f}'.format(*cmd))
# print('***********************************')
mov = self.gait['crawl'].command(cmd)
if mov:
self.robot.move(mov)
class SimpleQuadruped(object):
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 run(self):
while True:
if self.js.valid:
ps4 = self.js.get()
x, y = ps4['leftStick']
rz, _ = ps4['rightStick']
cmd = (x, y, rz)
else:
cmd = (1, 0, 0)
# read ahrs
if self.imu:
a, m, g = self.imu.read()
# roll, pitch, heading = d
# if (-90.0 > roll > 90.0) or (-90.0 > pitch > 90.0):
# print('Crap we flipped!!!')
# cmd = (0, 0, 0)
print('imu', a, m, g)
print('***********************************')
# print('* rest {:.2f} {:.2f} {:.2f}'.format(*leg))
# print('ahrs[deg]: roll {:.2f} pitch: {:.2f} yaw: {:.2f}'.format(d[0], d[1], d[2]))
print('* cmd {:.2f} {:.2f} {:.2f}'.format(*cmd))
# print('***********************************')
mov = self.gait['crawl'].command(cmd)
if mov:
self.robot.move(mov)
#!/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:
