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