コード例 #1
0
ファイル: ahrs.py プロジェクト: michaelmegliola/BeagleQuad
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)
コード例 #2
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)
コード例 #3
0
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"))
コード例 #4
0
ファイル: imu.py プロジェクト: SiChiTong/quadruped-AX12
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()
コード例 #5
0
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
コード例 #6
0
ファイル: imu.py プロジェクト: Xianmuyingyue/ins_nav
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()
コード例 #7
0
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)
コード例 #8
0
ファイル: Hardware.py プロジェクト: DFEC-R2D2/r2d2
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)
コード例 #9
0
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()
コード例 #10
0
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)
コード例 #11
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)
コード例 #12
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)
コード例 #13
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 ...')
コード例 #14
0
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')
コード例 #15
0
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 ...')
コード例 #16
0
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
コード例 #17
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)
    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('')
コード例 #18
0
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('')
コード例 #19
0
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)
コード例 #20
0
#!/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,
        ))
コード例 #21
0
    # 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))
コード例 #22
0
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.')
コード例 #23
0
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)
コード例 #24
0
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 ***")
コード例 #25
0
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())
コード例 #26
0
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()