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)
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 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 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 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 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 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)
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 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 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 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 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 ...')
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
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 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)
#!/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,
))
# if not lidar.open(port):
# exit(1)
start = time.time()
try:
# while(True):
for cnt in range(1000):
loop_start = time.time()
if cnt % 20 == 0:
print("\n{}\n".format(cnt))
else:
print(".", end='')
d = imu.get()
data['imu'].append(Data_ts(d, time.time() - start))
# d = lidar.capture()
# print(d)
# data['lidar'].append(Data_ts(d, time.time()-start))
img = cam.read()
while img is None:
print("cv2 crap")
time.sleep(1)
img = cam.read()
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
d = img.tobytes()
data['camera'].append(Data_ts(d, time.time() - start))
class NXP9DoF:
'''
Reads from an Adafruit NXP 9-DoF FXOS8700 + FXAS21002 sensor.
Note that this has been calibrated based on the orientation of the
NXP9DoF board as installed on the KR01 robot, with the Adafruit
logo on the chip (mounted horizontally) as follows:
Fore
_---------------
| * |
| |
| |
Port | | Starboard
| |
----------------
Aft
If you mount the chip in a different orientation you will likely
need to multiply one or more of the axis by -1.0.
Depending on where you are on the Earth, true North changes because
the Earth is not a perfect sphere. You can get the declination angle
for your location from:
http://www.magnetic-declination.com/
E.g., for Pukerua Bay, New Zealand:
Latitude: 41° 1' 57.7" S
Longitude: 174° 53' 11.8" E
PUKERUA BAY
Magnetic Declination: +22° 37'
Declination is POSITIVE (EAST)
Inclination: 66° 14'
Magnetic field strength: 55716.5 nT
E.g., for Wellington, New Zealand:
Latitude: 41° 17' 11.9" S
Longitude: 174° 46' 32.1" E
KELBURN
Magnetic Declination: +22° 47'
Declination is POSITIVE (EAST)
Inclination: 66° 28'
Magnetic field strength: 55863.3 nT
'''
# permitted error between Euler and Quaternion (in degrees) to allow setting value
ERROR_RANGE = 5.0 # was 3.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 get_imu(self):
return self._imu
# ..........................................................................
def get_fxos(self):
return self._fxos
# ..........................................................................
def get_fxas(self):
return self._fxas
# ..........................................................................
def enable(self):
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=NXP9DoF._read,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def get_heading(self):
return self._heading
# ..........................................................................
def get_pitch(self):
return self._pitch
# ..........................................................................
def get_roll(self):
return self._roll
# ..........................................................................
def is_calibrated(self):
return self._is_calibrated
# ..........................................................................
@staticmethod
def _in_range(p, q):
return p >= (q - NXP9DoF.ERROR_RANGE) and p <= (q +
NXP9DoF.ERROR_RANGE)
# ..........................................................................
def imu_enable(self):
'''
Enables the handbuilt IMU on a 20Hz loop.
'''
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=NXP9DoF._handbuilt_imu,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def _handbuilt_imu(self):
'''
The function that reads sensor values in a loop. This checks to see
if the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable for heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
'''
self._log.info('starting sensor read...')
_heading = None
_pitch = None
_roll = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
# grab data at 20Hz
_rate = Rate(1)
header = 90
print('-' * header)
# print("| {:17} | {:20} |".format("Accels [g's]", " IMU Accels"))
print("| {:17} |".format("Mag [xyz]"))
while not self._closed:
if self._enabled:
accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2
gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
a, m, g = self._imu.get()
mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
mag_x_g = m[0] * 100.0
mag_y_g = m[1] * 100.0
mag_z_g = m[2] * 100.0
# rate_gyro_x_dps = Convert.rps_to_dps(gyro_x)
# rate_gyro_y_dps = Convert.rps_to_dps(gyro_y)
# rate_gyro_z_dps = Convert.rps_to_dps(gyro_z)
# mag_x_g = mag_x * 100.0
# mag_y_g = mag_y * 100.0
# mag_z_g = mag_z * 100.0
heading = 180.0 * math.atan2(mag_y_g, mag_x_g) / math.pi
print(Fore.CYAN + '| x{:>6.3f} y{:>6.3f} z{:>6.3f} |'.format( mag_x_g, mag_y_g, mag_z_g) \
+ Style.BRIGHT + '\t{:>5.2f}'.format(heading) + Style.RESET_ALL)
# a, m, g = self._imu.get()
# print(Fore.CYAN + '| {:>6.3f} {:>6.3f} {:>6.3f} | {:>6.3f} {:>6.3f} {:>6.3f} |'.format( accel_x, accel_y, accel_z, a[0], a[1], a[2]) + Style.RESET_ALL)
_rate.wait()
# + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
# time.sleep(0.50)
# mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
# gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
# header = 90
# print('-'*header)
# print("| {:17} | {:20} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading"))
# print('-'*header)
# for _ in range(10):
# a, m, g = self._imu.get()
# r, p, h = self._imu.getOrientation(a, m)
# deg = Convert.to_degrees(h)
# print(Fore.CYAN + '| {:>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])
# + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
# time.sleep(0.50)
# print('-'*header)
# print(' uT: micro Tesla')
# print(' g: gravity')
# print('dps: degrees per second')
# print('')
#
# print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\
# + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\
# + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL)
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
# ..........................................................................
def _read(self):
'''
The function that reads sensor values in a loop. This checks to see
if the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable for heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
'''
self._log.info('starting sensor read...')
_heading = None
_pitch = None
_roll = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
# grab data at 10Hz
rate = Rate(10)
while not self._closed:
if self._enabled:
header = 91
print(Fore.CYAN + ('-' * header) + Style.RESET_ALL)
print(Fore.CYAN + "| {:17} | {:20} | {:20} | {:21} |".format(
"Accels [g's]", " Magnet [uT]", "Gyros [dps]",
"Roll, Pitch, Heading") + Style.RESET_ALL)
print(Fore.CYAN + ('-' * header) + Style.RESET_ALL)
for _ in range(10):
a, m, g = self._imu.get()
r, p, h = self._imu.getOrientation(a, m)
deg = Convert.to_degrees(h)
# self._log.info(Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h) + Style.RESET_ALL)
print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} '.format(
a[0], a[1], a[2]) + Fore.YELLOW +
'| {:>6.1f} {:>6.1f} {:>6.1f} '.format(
m[0], m[1], m[2]) + Fore.MAGENTA +
'| {:>6.1f} {:>6.1f} {:>6.1f} '.format(
g[0], g[1], g[2]) + Fore.GREEN +
'| {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) +
Style.RESET_ALL)
time.sleep(0.50)
print('-' * header)
print(' uT: micro Tesla')
print(' g: gravity')
print('dps: degrees per second')
print('')
# accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2
# mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
# gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
#
# print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\
# + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\
# + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL)
rate.wait()
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
# ..........................................................................
def disable(self):
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def close(self):
self._closed = True
self._log.info('closed.')
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)
class Create2(object):
bot = None
imu = None
camera = None
sensors = {}
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 __del__(self):
# self.bag.close()
self.bot.drive_direct(0, 0)
self.current_mode = 'idle'
print('Create2 robot is exiting ...')
def setMode(self, mode):
if mode in self.modes:
self.current_mode = mode
else:
raise Exception(
'Create2::setMode selected mode is invalid: {}'.format(mode))
def run(self):
self.bot.start()
self.bot.safe()
# self.bot.full()
# self.setMode('js')
self.setMode('demo')
# self.setMode('idle')
# self.setMode('auto')
# self.setMode('sensors')
while True:
self.get_sensors()
self.printInfo()
# control roobma
self.modes[self.current_mode].go(self.sensors)
# time.sleep(0.05)
def processImage(self, img):
pass
def printInfo(self):
sensors = self.sensors['create']
imu = self.sensors['imu']
if sensors is None or imu is None:
print('No valid sensor info')
return
a, m, g = imu
now = time.time()
dt = self.last_time - now
beta = 0.5
q = self.ahrs.updateAGM(a, m, g, beta, dt)
r, p, y = quat2euler(q)
# self.data['r'].push(r)
# self.data['p'].push(p)
self.data['y'].push(y)
self.last_time = now
ir = []
for i in [36, 37, 38, 39, 40, 41]:
ir.append(sensors[i])
cliff = []
for i in [20, 21, 22, 23]:
cliff.append(sensors[i])
po = [
'--------------------------------------------------------',
' Light Bumper: {:6} {:6} {:6} L| {:6} |R {:6} {:6} {:6}'.format(
sensors.light_bumper_left,
sensors.light_bumper_front_left,
sensors.light_bumper_center_left,
sparkline.sparkify(ir).encode('utf-8'),
# '',
sensors.light_bumper_center_right,
sensors.light_bumper_front_right,
sensors.light_bumper_right),
' Cliff: {:6} {:6} L| {:4} |R {:6} {:6}'.format(
sensors.cliff_left_signal,
sensors.cliff_front_left_signal,
sparkline.sparkify(cliff).encode('utf-8'),
# '',
sensors.cliff_front_right_signal,
sensors.cliff_right_signal),
' Encoders: {:7} L|R {:7}'.format(sensors.encoder_counts_left,
sensors.encoder_counts_right),
' Distance Delta: {:8} mm Total: {:10.1f} m'.format(
sensors.distance, self.distance),
# ' Yaw: {:8.1f} {:30} degrees'.format(self.data['y'].get_last(), self.data['y'].spark()),
'--------------------------------------------------------',
' Power: {:6} mAhr [{:3} %]'.format(
sensors.battery_charge,
int(100.0 * sensors.battery_charge /
sensors.battery_capacity)),
' Voltage: {:7.1f} V Current: {:7.1f} A'.format(
sensors.voltage / 1000, sensors.current / 1000)
]
for s in po:
print(s)
def get_sensors(self):
cr = self.bot.get_sensors()
self.sensors['create'] = cr
# self.bag.push('create', cr)
imu = self.imu.get()
self.sensors['imu'] = imu
# self.bag.push('imu', imu)
self.distance += cr.distance / 1000.0
# if 'camera' in self.keys:
self.sensors['camera'] = None
ret, img = self.camera.read()
if ret:
self.sensors['camera'] = img
else:
print("*** No camera image ***")
class IMU_FXOS8700:
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 get(self):
a, m, g = self.imu.get()
g = np.subtract(g, self.g_cal)
return a, m, g
def get_a(self):
if self.last_a == None:
dt = 0.0
self.last_a = time.time()
else:
t = time.time()
dt = t - self.last_a
self.last_a = t
return self.get()[ACCEL], dt
def get_g(self):
if self.last_g == None:
dt = 0.0
self.last_g = time.time()
else:
t = time.time()
dt = t - self.last_g
self.last_g = t
return self.get()[GYRO], dt
def get_attitude(self):
if self.last_ahrs == None:
dt = 0.0
self.last_ahrs = time.time()
else:
t = time.time()
dt = t - self.last_ahrs
self.last_ahrs = t
a, m, g = self.imu.get()
pitch = (np.arctan2(a[X], np.sqrt(a[Y]**2 + a[Z]**2)) * 180.0) / np.pi
roll = (np.arctan2(-a[Y], a[Z]) * 180.0) / np.pi
return [pitch, roll, 0.0], dt # heading is unknown
def calibrate(self):
print('Calibrating gyro...')
i = 0
while i < 400:
a, m, g = self.imu.get()
self.g_cal = np.add(g, self.g_cal)
i += 1
if not self.stationary() or not self.level():
print('Motion or tilt detected... restarting calibration')
self.g_cal = [0, 0, 0]
i = 0
self.g_cal = np.divide(self.g_cal, i)
print('Gyro calibration:', self.g_cal)
def stationary(self):
a, m, g = self.get()
if self._z_accel == None:
self._z_accel = a[X]
return True
elif abs(a[X] - self._z_accel) > ACCEL_LIMIT:
self._z_accel = None
return False
else:
return True
def level(self):
a, m, g = self.get()
lim = ACCEL_LIMIT * 3.0
return abs(a[X]) < lim and abs(a[Y]) < lim and abs(a[Z] - 1.0) < lim
def __str__(self):
return str(self.get())
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()