def start(self): print('starting / rcpy initial state =', Quad.RCPY_STATES[rcpy.get_state()]) rcpy.set_state(rcpy.RUNNING) self.ahrs.start() self.escs.start() self.altimeter.start() print('starting / rcpy final state =', Quad.RCPY_STATES[rcpy.get_state()])
def thread_function(imu, name): # make sure the thread will terminate while rcpy.get_state() != rcpy.EXITING: # read to synchronize with imu data = imu.read() # handle other states if rcpy.get_state() == rcpy.RUNNING: # do things print('Hi from thread {}!'.format(name))
def __init__(self): if rcpy.get_state() != rcpy.RUNNING: rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_dmp=True, dmp_sample_rate=100, enable_fusion=True, enable_magnetometer=True) self.i = 0
def test1(): # initialize rcpy.initialize() assert rcpy.get_state() == rcpy.IDLE # clean up rcpy.cleanup() assert rcpy.get_state() == rcpy.EXITING # initialize again rcpy.initialize() assert rcpy.get_state() == rcpy.IDLE # set state rcpy.set_state(rcpy.PAUSED) assert rcpy.get_state() == rcpy.PAUSED # set state rcpy.set_state(rcpy.RUNNING) assert rcpy.get_state() == rcpy.RUNNING
def main(): # defaults enable_magnetometer = False show_compass = False show_gyro = False show_accel = False show_quat = False show_tb = False sample_rate = 100 enable_fusion = False show_period = False newline = '\r' show_tb = True # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # magnetometer ? mpu9250.initialize(enable_dmp=True, dmp_sample_rate=sample_rate, enable_fusion=enable_fusion, enable_magnetometer=enable_magnetometer) try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: t0 = time.perf_counter() data = mpu9250.read() t1 = time.perf_counter() dt = t1 - t0 t0 = t1 print(data) # print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' # .format(data['tb']), end='') # no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
def sweep(self, angle): rcpy.set_state(rcpy.RUNNING) srvo = servo.Servo(self.channel) duty = self.angle_to_duty(angle) clck = clock.Clock(srvo, self.period) try: servo.enable() clck.start() d = 0 direction = 1 duty = math.fabs(duty) delta = duty / 100 while rcpy.get_state() != rcpy.EXITING: # increment duty d = d + direction * delta # end of range? if d > duty or d < -duty: direction = direction * -1 if d > duty: d = duty else: d = -duty srvo.set(d) self.duty = d msg = servolcm() msg.duty = self.duty msg.angle = (self.duty) msg.timestamp = datetime.strftime(datetime.now(), datetime_format) self.lc.publish("servo_data", msg.encode()) time.sleep(.02) # handle ctrl c exit except KeyboardInterrupt: pass finally: clck.stop() servo.disable()
def run(self): self.run = True while rcpy.get_state() != rcpy.EXITING and self.run: try: evnt = self.input.high_or_low(self.debounce, self.timeout, self.pipe) if evnt is not None: evnt = 1 << evnt if evnt & self.event: # fire callback self.action(evnt) except InputTimeout: self.run = False
def run(self): self.run = True while rcpy.get_state() != rcpy.EXITING and self.run: # Acquire condition self.condition.acquire() # Setup timer self.timer = threading.Timer(self.period, self._run) self.timer.start() # Wait self.condition.wait() # and release self.condition.release()
def servo_sweep(self, direction, d=0): # keep running while rcpy.get_state() != rcpy.EXITING: # increment duty d = d + direction * self.delta # end of range? self.duty = math.fabs(self.duty) if d > self.duty: d = self.duty direction = direction * -1 elif d < -self.duty: d = -self.duty direction = direction * -1 self.internal.set(d) time.sleep(.02)
def high_or_low(self, debounce = 0, timeout = None, pipe = None): # repeat until event is detected while rcpy.get_state() != rcpy.EXITING: # read event event = read(self.pin, timeout, pipe) # debounce k = 0 value = event while k < debounce and value == event: time.sleep(DEBOUNCE_INTERVAL/1000) value = get(self.pin) k += 1 # check value if value == event: return value
def run(self, direction): if self.duty != 0: if not self.sweep: print('Setting servo {} to {} duty'.format(self.channel, self.duty)) self.internal.set(self.duty) else: print('Sweeping servo {} to {} duty'.format(self.channel, self.duty)) else: self.sweep = False try: # enable servos servo.enable() # start clock self.clck.start() # sweep if self.sweep: self.servo_sweep(direction) else: # keep running while rcpy.get_state() != rcpy.EXITING: time.sleep(1) except KeyboardInterrupt: # stop clock self.clck.stop() # disable servos servo.disable() finally: # stop clock self.clck.stop() # disable servos servo.disable() # say bye print("\nBye Beaglebone!")
def read(self): try: # keep running while True: if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |' ' {3:6.1f}').format(data['accel'], data['gyro'], data['mag'], temp), end='') return data except KeyboardInterrupt: # Catch Ctrl-C pass finally: print("\nBye BeagleBone!")
def high_or_low(self, debounce=0, timeout=0, pipe=None): # repeat until event is detected while rcpy.get_state() != rcpy.EXITING: # read event value = self.read(timeout, pipe) # debounce k = 0 current_value = value while k < debounce and value == current_value: time.sleep(DEBOUNCE_INTERVAL / 1000) current_value = self.get() k += 1 # check value if value == current_value: # return value return value
def trackingDrive(): width = 240 # Resized image width. This is the image width in pixels. size_h = 160 # Resized image height. This is the image height in pixels. tc = 40 # Too Close - Maximum pixel size of object to track tf = 15 # Too Far - Minimum pixel size of object to track tp = 25 # Target Pixels - Target size of object to track band = 25 #range of x considered to be centered x = 0 # will describe target location left to right y = 0 # will describe target location bottom to top radius = 0 # estimates the radius of the detected target duty_l = 0 # initialize motor with zero duty cycle duty_r = 0 # initialize motor with zero duty cycle print("initializing rcpy...") rcpy.set_state(rcpy.RUNNING) # initialize rcpy print("finished initializing rcpy.") try: while rcpy.get_state() != rcpy.EXITING: if rcpy.get_state() == rcpy.RUNNING: scale_t = 1 # a scaling factor for speeds scale_d = 1.3 # a scaling factor for speeds motor_r = 2 # Right Motor assigned to #2 motor_l = 1 # Left Motor assigned to #1 x, y, radius = trackTarget.colorTarget( color_range) # Get properties of circle around shape if x != None: # If more than 0 closed shapes exist # handle centered condition if x > ((width / 2) - (band / 2)) and x < ( (width / 2) + (band / 2)): # If center point is centered dir = "driving" if (radius - tp) >= 1: # Too Close case = "too close" duty = -1 * ((radius - tp) / (tc - tp)) dutyF = 0 elif (radius - tp) < -1: # Too Far case = "too far" duty = 1 - ((radius - tf) / (tp - tf)) duty = scale_d * duty dutyF = 0 else: case = "good" duty = 0 dutyF = -0.5 duty_r = duty duty_l = duty else: case = "turning" dutyF = 0 duty_l = round((x - 0.5 * width) / (0.5 * width), 2) # Duty Left duty_l = duty_l * scale_t duty_r = round((0.5 * width - x) / (0.5 * width), 2) # Duty Right duty_r = duty_r * scale_t # Keep duty cycle within range if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 if duty_r < .3 and duty_r > 0: duty_r = .3 elif duty_r < 0 and duty_r > -.3: duty_r = -.3 if duty_l < .3 and duty_l > 0: duty_l = .3 elif duty_l < 0 and duty_l > -0.3: duty_l = -0.3 # Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) print(case, "\tradius: ", round(radius, 1), "\tx: ", round(x, 0), "\t\tL: ", duty_l, "\tR: ", duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) motor.set(3, dutyF) log.stringTmpFile(str(duty_l), "leftMotor.txt") log.stringTmpFile(str(duty_r), "rightMotor.txt") log.stringTmpFile(str(radius), "radiusOfComputerVision.txt") log.stringTmpFile(str(x), "xValue.txt")
print("Press Ctrl-C to exit") # header,, print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)') i = 0 avg = 0 save = [] sigma = 0 cov = 0 try: # keep running while True: while i < 100: if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read_accel_data() save.append(data[1]) avg = avg + data[1] i = i + 1 avg = avg / 100 print('doing') time.sleep(0.01) print('done') for i in range(0, len(save)): sigma = sigma + ((save[i] - avg)**2) stdv = math.sqrt(sigma / 100) cov = stdv**2 cov = [[cov, 0, 0], [0, cov, 0], [0, 0, cov]] print('done1')
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "bfhsd:m:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_motors: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults duty = 0.0 channel = 0 sweep = False brk = False free = False steps = 20 period = 4 for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-d": duty = float(a) elif o in "-m": channel = int(a) elif o == "-s": sweep = True elif o == "-b": brk = True elif o == "-f": free = True elif o == "-t": period = float(a) elif o == "-n": steps = int(a) else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # set motor duty (only one option at a time) if brk: print('Breaking motor {}'.format(channel)) motor.set_brake(channel) sweep = False elif free: print('Setting motor {} free'.format(channel)) motor.set_free_spin(channel) sweep = False elif duty != 0: if not sweep: print('Setting motor {} to {} duty'.format(channel, duty)) motor.set(channel, duty) else: print('Sweeping motor {} to {} duty'.format(channel, duty)) else: sweep = False # message print("Press Ctrl-C to exit") try: # sweep if sweep: d = 0 direction = 1 duty = math.fabs(duty) delta = duty/steps dt = period/steps/2 # keep running while rcpy.get_state() != rcpy.EXITING: # running if rcpy.get_state() == rcpy.RUNNING: # increment duty d = d + direction * delta motor.set(channel, d) # end of range? if d > duty or d < -duty: direction = direction * -1 elif rcpy.get_state() == rcpy.PAUSED: # set motors to free spin motor.set_free_spin(channel) d = 0 # sleep some time.sleep(dt) # or do nothing else: # keep running while rcpy.get_state() != rcpy.EXITING: # sleep some time.sleep(1) except KeyboardInterrupt: # handle what to do when Ctrl-C was pressed pass finally: # say bye print("\nBye Beaglebone!")
import rcpy.motor as motor # defaults duty = 0.3 rcpy.set_state(rcpy.RUNNING) print("Press Ctrl-C to exit") try: d = 0 direction = 1 delta = duty/10 while rcpy.get_state() != rcpy.EXITING: # keep running # increment duty d = d + direction * delta motor.set(2, d) motor.set(3, d) if d > duty or d < -duty: # end of range? direction = direction * -1 time.sleep(.1) # sleep some except KeyboardInterrupt: # handle what to do when Ctrl-C was pressed pass finally:
def main(): camera = cv2.VideoCapture(camera_input) # Define camera variable camera.set(3, size_w) # Set width of images that will be retrived from camera camera.set( 4, size_h) # Set height of images that will be retrived from camera tc = 70 # Too Close - Maximum pixel size of object to track tf = 6 # Too Far - Minimum pixel size of object to track tp = 65 # Target Pixels - Target size of object to track band = 50 #range of x considered to be centered x = 0 # will describe target location left to right y = 0 # will describe target location bottom to top radius = 0 # estimates the radius of the detected target duty_l = 0 # initialize motor with zero duty cycle duty_r = 0 # initialize motor with zero duty cycle print("initializing rcpy...") rcpy.set_state(rcpy.RUNNING) # initialize rcpy print("finished initializing rcpy.") try: while rcpy.get_state() != rcpy.EXITING: if rcpy.get_state() == rcpy.RUNNING: scale_t = 1.3 # a scaling factor for speeds scale_d = 1.3 # a scaling factor for speeds motor_r = 2 # Right Motor assigned to #2 motor_l = 1 # Left Motor assigned to #1 ret, image = camera.read() # Get image from camera height, width, channels = image.shape # Get size of image if not ret: break if filter == 'RGB': # If image mode is RGB switch to RGB mode frame_to_thresh = image.copy() else: frame_to_thresh = cv2.cvtColor( image, cv2.COLOR_BGR2HSV) # Otherwise continue reading in HSV thresh = cv2.inRange( frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) # Find all pixels in color range kernel = np.ones((5, 5), np.uint8) # Set gaussian blur strength. mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # Apply gaussian blur mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) cnts = cv2.findContours( mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] # Find closed shapes in image center = None # Create variable to store point if len(cnts) > 0: # If more than 0 closed shapes exist c = max(cnts, key=cv2.contourArea ) # Get the properties of the largest circle ((x, y), radius) = cv2.minEnclosingCircle( c) # Get properties of circle around shape radius = round(radius, 2) # Round radius value to 2 decimals x = int(x) # Cast x value to an integer M = cv2.moments(c) # Gets area of circle contour center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]) ) # Get center x,y value of circle # handle centered condition if x > ((width / 2) - (band / 2)) and x < ( (width / 2) + (band / 2)): # If center point is centered dir = "driving" if radius >= tp: # Too Close case = "too close" duty = -1 * ((radius - tp) / (tc - tp)) elif radius < tp: # Too Far case = "too far" duty = 1 - ((radius - tf) / (tp - tf)) duty = scale_d * duty duty_r = duty duty_l = duty else: case = "turning" duty_l = round((x - 0.5 * width) / (0.5 * width), 2) # Duty Left duty_l = duty_l * scale_t duty_r = round((0.5 * width - x) / (0.5 * width), 2) # Duty Right duty_r = duty_r * scale_t # Keep duty cycle within range if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 # Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) print(case, "\tradius: ", round(radius, 1), "\tx: ", round(x, 0), "\t\tL: ", duty_l, "\tR: ", duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r)
return (angle0) # returns the current angle for the left and right encoder ######################################################################################### print("initializing rcpy...") rcpy.set_state(rcpy.RUNNING) print("finished initializing rcpy.") try: while rcpy.get_state() != rcpy.EXITING: # Checks if you're in the correct state if rcpy.get_state() == rcpy.RUNNING: # check rcpy is running, then continue program x0 = x1 # store previous x reading enc1 = read_encoder_angle(encL) # grab a new encoder reading x1 = enc1/360*dt_rev # left-hand wheel distance = degrees * distance traveled per rev dx1 = -1*(x1-x0) # calculate change in distance (reversed magnitude for left hand) if dx1 < -0.001: # if the change is negative (over 1mm,) the encoder has rolled over dx1 = dx1 + dt_rev #add 1 revolution if the encoder rolls over t0 = t1 # store t1 to t0 t1 = time.time() # capture time dt0 = dt1 # store previous delta in time dt1 = t1-t0 # delta in time v0 = v1 # store previous velocity
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "he:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_encoders: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults channel = 0 for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-e": channel = int(a) else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # message print("Press Ctrl-C to exit") # header if channel == 0: print(' E1 | E2 | E3 | E4') else: print(' E{}'.format(channel)) try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: if channel == 0: # read all encoders e1 = encoder.get(1) e2 = encoder.get(2) e3 = encoder.get(3) e4 = encoder.get(4) print('\r {:+6d} | {:+6d} | {:+6d} | {:+6d}'.format( e1, e2, e3, e4), end='') else: # read one encoder e = encoder.get(channel) print('\r {:+6d}'.format(e), end='') # sleep some time.sleep(.5) except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
def read(pin, timeout = None, pipe = None): # create pipe if necessary destroy_pipe = False if pipe is None: pipe = rcpy.create_pipe() destroy_pipe = True # open stream filename = SYSFS_GPIO_DIR + '/gpio{}/value'.format(pin) with open(filename, 'rb', buffering = 0) as f: # create poller poller = select.poll() f.read() poller.register(f, select.POLLPRI | select.POLLHUP | select.POLLERR) # listen to state change as well state_r_fd, state_w_fd = pipe poller.register(state_r_fd, select.POLLIN | select.POLLHUP | select.POLLERR) while rcpy.get_state() != rcpy.EXITING: # wait for events if timeout: # can fail if timeout is given events = poller.poll(timeout) if len(events) == 0: # destroy pipe if destroy_pipe: rcpy.destroy_pipe(pipe) # raise timeout exception raise InputTimeout('Input did not change in more than {} ms'.format(timeout)) else: # timeout = None, never fails events = poller.poll() for fd, flag in events: # state change if fd is state_r_fd: # get state state = int(os.read(state_r_fd, 1)) if state == rcpy.EXITING: # exit! if destroy_pipe: rcpy.destroy_pipe(pipe) return # input event if fd is f.fileno(): # Handle inputs if flag & (select.POLLIN | select.POLLPRI): # destroy pipe if destroy_pipe: rcpy.destroy_pipe(pipe) # return read value return get(pin) elif flag & (select.POLLHUP | select.POLLERR): # destroy pipe if destroy_pipe: rcpy.destroy_pipe(pipe) # raise exception raise Exception('Could not read pin {}'.format(pin)) # destroy pipe if destroy_pipe: rcpy.destroy_pipe(pipe)
def main(): # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "hm", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_imu: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults enable_magnetometer = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o == "-m": enable_magnetometer = True else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # no magnetometer mpu9250.initialize(enable_magnetometer = enable_magnetometer) # message print("try 'python rcpy_test_imu -h' to see other options") print("Press Ctrl-C to exit") # header print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') if enable_magnetometer: print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)') try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read() if enable_magnetometer: print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |' ' {3:6.1f}').format(data['accel'], data['gyro'], data['mag'], temp), end='') else: print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' ' {2:6.1f}').format(data['accel'], data['gyro'], temp), end='') # sleep some time.sleep(.5) except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
# Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) print(case, "\tradius: ", round(radius, 1), "\tx: ", round(x, 0), "\t\tL: ", duty_l, "\tR: ", duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) elif rcpy.get_state() == rcpy.PAUSED: pass except KeyboardInterrupt: # condition added to catch a "Ctrl-C" event and exit cleanly rcpy.set_state(rcpy.EXITING) pass finally: rcpy.set_state(rcpy.EXITING) print("Exiting Color Tracking.") # exiting program will automatically clean up cape if __name__ == '__main__':
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "he:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_encoders: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults channel = 0 for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-e": channel = int(a) else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # message print("Press Ctrl-C to exit") # header if channel == 0: print(' E1 | E2 | E3 | E4') else: print(' E{}'.format(channel)) try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: if channel == 0: # read all encoders e1 = encoder.get(1) e2 = encoder.get(2) e3 = encoder.get(3) e4 = encoder.get(4) print('\r {:+6d} | {:+6d} | {:+6d} | {:+6d}'.format(e1,e2,e3,e4), end='') else: # read one encoder e = encoder.get(channel) print('\r {:+6d}'.format(e), end='') # sleep some time.sleep(.5) except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
rcpy.set_state(rcpy.RUNNING) # spinning wheel i = 0 spin = '-\|/' try: # keep running forever while True: # read to synchronize with imu data = imu.read() # running? if rcpy.get_state() == rcpy.RUNNING: # do things print('\r{}'.format(spin[i % len(spin)]), end='') i = i + 1 # paused? elif rcpy.get_state() == rcpy.PAUSED: # do other things pass # there is no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass
import rcpy.mpu9250 as mpu9250 rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_magnetometer = True) print("Press Ctrl-C to exit") # header print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)') try: # keep running while True: if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |' ' {3:6.1f}').format(data['accel'], data['gyro'], data['mag'], temp), end='') time.sleep(.5) # sleep some except KeyboardInterrupt: # Catch Ctrl-C pass
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "bfhsd:m:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_motors: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults duty = 0.0 channel = 0 sweep = False brk = False free = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-d": duty = float(a) elif o in "-m": channel = int(a) elif o == "-s": sweep = True elif o == "-b": brk = True elif o == "-f": free = True else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # set motor duty (only one option at a time) if brk: print('Breaking motor {}'.format(channel)) motor.set_brake(channel) sweep = False elif free: print('Setting motor {} free'.format(channel)) motor.set_free_spin(channel) sweep = False elif duty != 0: if not sweep: print('Setting motor {} to {} duty'.format(channel, duty)) motor.set(channel, duty) else: print('Sweeping motor {} to {} duty'.format(channel, duty)) else: sweep = False # message print("Press Ctrl-C to exit") try: # sweep if sweep: d = 0 direction = 1 duty = math.fabs(duty) delta = duty/20 # keep running while rcpy.get_state() != rcpy.EXITING: # running if rcpy.get_state() == rcpy.RUNNING: # increment duty d = d + direction * delta motor.set(channel, d) # end of range? if d > duty or d < -duty: direction = direction * -1 elif rcpy.get_state() == rcpy.PAUSED: # set motors to free spin motor.set_free_spin(channel) d = 0 # sleep some time.sleep(.1) # or do nothing else: # keep running while rcpy.get_state() != rcpy.EXITING: # sleep some time.sleep(1) except KeyboardInterrupt: # handle what to do when Ctrl-C was pressed pass finally: # say bye print("\nBye Beaglebone!")
elbow_clck.start() pitch_clck.start() roll_clck.start() grabber_clck.start() dock_arm() time.sleep(1) ready_arm() time.sleep(0.5) while True: if rcpy.get_state() == rcpy.RUNNING: try: # line = serial_input.readline() # print(line) gyro_data = imu.read() data, addr = sock.recvfrom(bufferSize) gyro_x = -180 * (gyro_data.get("quat")[2]) gyro_y = -180 * (gyro_data.get("quat")[1]) gyro_z = (gyro_data.get("quat")[0]) if len(data) == 0:
def MotorL(speed): # takes argument in range [-1,1] motor.set(motor_l, speed) def MotorR(speed): # takes argument in range [-1,1] motor.set(motor_r, speed) def diode(state, channel): # takes argument in range [0,1] np.clip(state, 0, 1) # limit the output, disallow negative voltages motor.set(channel, state) def accy(state, channel): # takes argument in range [-1,1] motor.set(channel, state) if __name__ == "__main__": while rcpy.get_state() != rcpy.EXITING: # exit loop if rcpy not ready if rcpy.get_state() == rcpy.RUNNING: # execute loop when rcpy is ready print("motors.py: driving fwd") MotorL( 1 ) # gentle speed for testing program. 0.3 PWM may not spin the wheels. MotorR(1) time.sleep(4) # run fwd for 4 seconds print("motors.py: driving reverse") MotorL(-0.6) MotorR(-0.6) time.sleep(4) # run reverse for 4 seconds
def main(): camera = cv2.VideoCapture(camera_input) # Define camera variable camera.set(3, size_w) # Set width of images that will be retrived from camera camera.set(4, size_h) # Set height of images that will be retrived from camera tc = 300 # Too Close - Maximum pixel size of object to track tf = 6 # Too Far - Minimum pixel size of object to track tp = 200 # Target Pixels - Target size of object to track band = 130 #range of x considered to be centered x = 0 # will describe target location left to right y = 0 # will describe target location bottom to top radius = 0 # estimates the radius of the detected target duty_l = 0 # initialize motor with zero duty cycle duty_r = 0 # initialize motor with zero duty cycle maxCount = 40 currentCount = 0 d = .6 reached = False print("initializing rcpy...") rcpy.set_state(rcpy.RUNNING) # initialize rcpy print("finished initializing rcpy.") f = 0 InitFiles() delta_x = 0 delta_y = 0 x_dot = 0 try: while rcpy.get_state() != rcpy.EXITING: t = time.time() r = 0 if rcpy.get_state() == rcpy.RUNNING: scale_t = .3 # a scaling factor for speeds scale_d = 1 # a scaling factor for speeds motor_r = 2 # Right Motor assigned to #2 motor_l = 1 # Left Motor assigned to #1 direction = 0 found = False #image = cv2.imread("image2.bmp") ret, image = camera.read() # Get image from camera cv2.imwrite('image2.jpg',image) height, width, channels = image.shape # Get size of image #if not ret: # break if filter == 'RGB': # If image mode is RGB switch to RGB mode frame_to_thresh = image.copy() else: frame_to_thresh = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # Otherwise continue reading in HSV thresh = cv2.inRange(frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) # Find all pixels in color range time.sleep(0.02) cv2.imwrite('thresh.jpg',thresh) kernel = np.ones((5,5),np.uint8) # Set gaussian blur strength. mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # Apply gaussian blur mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] # Find closed shapes in image center = None # Create variable to store point if len(cnts) > 0: # If more than 0 closed shapes exist c = max(cnts, key=cv2.contourArea) # Get the properties of the largest circle ((x, y), radius) = cv2.minEnclosingCircle(c) # Get properties of circle around shape radius = round(radius, 2) # Round radius value to 2 decimals r = radius x = int(x) # Cast x value to an integer M = cv2.moments(c) # Gets area of circle contour center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # Get center x,y value of circle h = heading.getDegrees() # handle centered condition if x > ((width/2)-(band/2)) and x < ((width/2)+(band/2)): # If center point is centered dir = "driving" found = True if abs(radius - tp)<10: reached = True servo.move1(1) duty = 0 elif radius < tp: # Too Far case = "too far" servo.move1(-1) duty = 1 - ((radius - tf)/(tp - tf)) duty = scale_d * duty reached = False duty_r = duty duty_l = duty else: reached = False case = "turning" duty_l = round((x-0.5*width)/(0.5*width),2) # Duty Left duty_l *= scale_t duty_r = round((0.5*width-x)/(0.5*width),2) # Duty Right duty_r *= scale_t limit = 0.3 if(abs(duty_l)<limit): duty_l*= limit/duty_l if(abs(duty_r)<limit): duty_r*= limit/duty_r file = open("radius.txt","w") file.write(str(reached)+","+ str(r)+"\n") file.close() # Keep duty cycle within range if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 # Round duty cycles duty_l = round(duty_l,2) duty_r = round(duty_r,2) direction = duty_r - duty_l case+= " "+str(direction) print(x_dot," ",case, "\tradius: ", round(radius,1), "\tx: ", round(x,0), "\t\tL: ", duty_l, "\tR: ", duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) else: direction = duty_r - duty_l print(currentCount, " ", direction) if(currentCount <maxCount): duty_l = d * scale_t duty_r = -d * scale_t currentCount += 1 else: d*= -1 currentCount = 0 file = open("data.txt","a") file.write(str(duty_r) +","+ str(duty_l)+"\n") file.close() # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) tend = time.time() del_t = tend - t length = x_dot*del_t delta_x += math.cos(h) * length delta_y += math.sin(h) * length log.uniqueFile(tend - t, "time.txt") x_dot = speed([duty_l, duty_r])[0] elif rcpy.get_state() == rcpy.PAUSED: pass
# Keep duty cycle within range for right wheel if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 # Keep duty cycle within range for left wheel if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 # Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) elif rcpy.get_state() == rcpy.EXITING: pass except KeyboardInterrupt: # condition added to catch a "Ctrl-C" event and exit cleanly rcpy.set_state(rcpy.EXITING) pass finally: rcpy.set_state(rcpy.EXITING) print("Exiting Color Tracking.") if __name__ == '__main__': main()
# create and start ButtonEvent mode_event = button.ButtonEvent(button.mode, button.ButtonEvent.PRESSED, target = mode_pressed, vargs = (blink_red, blink_green, rates)) mode_event.start() # welcome message print("Green and red LEDs should be flashing") print("Press button <MODE> to change the blink rate") print("Press button <PAUSE> to stop or restart blinking") print("Hold button <PAUSE> for 2 s to exit") try: while rcpy.get_state() != rcpy.EXITING: # this is a blocking call! if button.pause.pressed(): # pause pressed try: # this is a blocking call with a timeout! if button.pause.released(timeout = 2000): # released too soon! print("<PAUSE> pressed, toggle blinking") # toggle blinking
def getTemp(): temp = mpu9250.read_imu_temp() # returns just temperature (deg C) return(temp) def getMag(): mag = mpu9250.read_mag_data() # gets x,y,z mag values (microtesla) mag = np.round(mag, 1) # round values to 1 decimal return(mag) def getGyro(): gyro = mpu9250.read_gyro_data() # returns 3 axes gyro data (deg/s) gyro = np.round(gyro, 2) return(gyro) if __name__ == "__main__": while True: if rcpy.get_state() == rcpy.RUNNING: # verify the rcpy package is running myTemp = getTemp() myMag = getMag() myGyro = getGyro() myAccel = getAccel() print("mag (μT):", myMag, "\t accel(m/s^2):", myAccel) time.sleep(0.2)
def main(): camera = cv2.VideoCapture(camera_input) # Define camera variable camera.set(3, size_w) # Set width of images that will be retrived from camera camera.set( 4, size_h) # Set height of images that will be retrived from camera tc = 70 # Too Close - Maximum pixel size of object to track tf = 6 # Too Far - Minimum pixel size of object to track tp = 65 # Target Pixels - Target size of object to track band = 40 #range of x considered to be centered x = 0 # will describe target location left to right y = 0 # will describe target location bottom to top radius = 0 # estimates the radius of the detected target duty = 0 duty_l = 0 # initialize motor with zero duty cycle duty_r = 0 # initialize motor with zero duty cycle print("initializing rcpy...") rcpy.set_state(rcpy.RUNNING) # initialize rcpy print("finished initializing rcpy.") p = 0 #Boolean to stop until ___ reloading = 0 button = 0 shooting = 0 scale_t = .8 # a scaling factor for speeds scale_d = .8 # a scaling factor for speeds motor_S = 3 # Trigger Motor assigned to #3 motor_r = 2 # Right Motor assigned to #2 motor_l = 1 # Left Motor assigned to #1 state = 1 battery = 0 shots = 0 integrate = 0 ki = 0.001 error = 0 try: while p != 1: p = gamepad.getGP() p = p[7] print("waiting for input") while rcpy.get_state() != rcpy.EXITING: if p == 1: print(state) battery = bat.getDcJack() log.tmpFile(state, "state.txt") #States log.tmpFile(shots, "shots.txt") #Shots left log.tmpFile(radius, "radius.txt") #radius (distance from tartget) log.tmpFile(battery, "battery.txt") #battery voltage log.tmpFile(duty_l, "dcL.txt") #motor duty cycle L log.tmpFile(duty_r, "dcR.txt") #motor duty cycle R if rcpy.get_state() == rcpy.RUNNING: #Camera code ret, image = camera.read() # Get image from camera height, width, channels = image.shape # Get size of image if not ret: break if filter == 'RGB': # If image mode is RGB switch to RGB mode frame_to_thresh = image.copy() else: frame_to_thresh = cv2.cvtColor( image, cv2.COLOR_BGR2HSV ) # Otherwise continue reading in HSV thresh = cv2.inRange( frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) # Find all pixels in color range kernel = np.ones((5, 5), np.uint8) # Set gaussian blur strength. mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # Apply gaussian blur mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) cnts = cv2.findContours( mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[ -2] # Find closed shapes in image center = None # Create variable to store point #End of camera code if state == 1 and len( cnts) > 0: # If more than 0 closed shapes exist state = 2 if state == 2 and len(cnts) == 0: state = 1 if shooting == 1 and state == 2: state = 3 if state == 3 and reloading == 1: state = 4 if state == 4 and button == 1: state = 1 if state == 1: #case = "turning" duty_l = 1 duty_r = -1 integrate = 0 if state == 2 and len(cnts) > 0: #Case = Target Aquesition c = max(cnts, key=cv2.contourArea ) # Get the properties of the largest circle ((x, y), radius) = cv2.minEnclosingCircle( c) # Get properties of circle around shape radius = round(radius, 2) # Round radius value to 2 decimals print(radius) x = int(x) # Cast x value to an integer M = cv2.moments(c) # Gets area of circle contour center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]) ) # Get center x,y value of circle # handle centered condition if x > ((width / 2) - (band / 2)) and x < ( (width / 2) + (band / 2)): # If center point is centered if radius > 38: # Too Close #case = "too close" case = "Back Up" duty = -1.2 print("too close") shooting = 0 duty_l = duty duty_r = duty elif radius >= 34 and radius <= 38: case = "On target" duty = 0 shooting = 1 print("on target") duty_l = duty duty_r = duty elif radius < 34: # Too Far case = "Bruh where it at" duty = 1.2 duty = scale_d * duty print("too far") shooting = 0 duty_l = duty duty_r = duty else: #case = "turning" case = "Looking For Target" print(x) """ if x>50: duty_l = .2 duty_r = -.2 elif x<50: duty_l = -.2 duty_r = .2""" error = 50 - x integrate = integrate + error duty_l = round((x - 0.5 * width) / (0.5 * width) + ki * integrate, 2) # Duty Left #duty_l = duty_l*scale_t #duty_r = round((0.5*width-x)/(0.5*width),2) # Duty Right #duty_r = duty_r*scale_t print("turning") shooting = 0 print(duty_l) if state == 3: #State = Shooting motor.set(motor_l, 0) motor.set(motor_r, 0) print("Getting ready to shoot") motor.set(motor_S, -.45) time.sleep(.3) motor.set(motor_S, .45) time.sleep(.3) motor.set(motor_S, 0) print("Gotcha Bitch") reloading = 1 button = 0 if state == 4: reloading = 0 print("Waiting for reload, press A when done") button = gamepad.getGP() button = button[4] duty_l = 0 duty_r = 0 shots = shots + 1 # Keep duty cycle within range if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 duty_l = duty_l * scale_t duty_r = duty_r * scale_t # Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) print(duty_l) print(duty_r) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r)
def main(): print("PREPPING for Pickup") camera = cv2.VideoCapture(camera_input) # Define camera variable camera.set(3, size_w) # Set width of images that will be retrived from camera camera.set( 4, size_h) # Set height of images that will be retrived from camera # reduced tc value to 0, allows robot to move over tc = 90 # Too Close - Maximum pixel size of object to track tf = 6 # Too Far - Minimum pixel size of object to track tp = 65 # Target Pixels - Target size of object to track band = 50 # range of x considered to be centered x = 0 # will describe target location left to right y = 0 # will describe target location bottom to top radius = 0 # estimates the radius of the detected target duty_l = 0 # initialize motor with zero duty cycle duty_r = 0 # initialize motor with zero duty cycle #print("initializing RCPY...") rcpy.set_state(rcpy.RUNNING) # initialize the rcpy library #print("finished initializing rcpy.") # obtain color tracking phi dot values try: print("Initial Prep") while rcpy.get_state() != rcpy.EXITING: if rcpy.get_state() == rcpy.RUNNING: scale_t = 1.3 # a scaling factor for speeds scale_d = 1.3 # a scaling factor for speeds motor_r = 2 # Right Motor assigned to #2 motor_l = 1 # Left Motor assigned to #1 ret, image = camera.read() # Get image from camera height, width, channels = image.shape # Get size of image if not ret: break if filter == 'RGB': # If image mode is RGB switch to RGB mode frame_to_thresh = image.copy() else: frame_to_thresh = cv2.cvtColor( image, cv2.COLOR_BGR2HSV) # Otherwise continue reading in HSV thresh = cv2.inRange( frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) # Find all pixels in color range kernel = np.ones((5, 5), np.uint8) # Set gaussian blur strength. mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # Apply gaussian blur mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) cnts = cv2.findContours( mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] # Find closed shapes in image center = None # Create variable to store point # locate item center point "prepping for pickup" if len(cnts) > 0: # If more than 0 closed shapes exist c = max(cnts, key=cv2.contourArea ) # Get the properties of the largest circle ((x, y), radius) = cv2.minEnclosingCircle( c) # Get properties of circle around shape radius = round(radius, 2) # Round radius value to 2 decimals x = int(x) # Cast x value to an integer M = cv2.moments(c) # Gets area of circle contour center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]) ) # Get center x,y value of circle print("x: ", x, "\ty: ", y, "\tR: ", radius, "\tCenter: ", center) # handle centered condition if x > ((width / 2) - (band / 2)) and x < ( (width / 2) + (band / 2)): # If center point is centered magnet.em_on() #item centered to SCUTTLE center of usb camera "centered" print("Item Centered") # once SCUTTLE is center; drive towards metal items. "picking up" dir = "driving" if radius >= tp: # Too close case = "too close" duty = -1 * ((radius - tp) / (tc - tp)) print("slowly approach") elif radius < tp: # Too far case = "too far" duty = 1 - ((radius - tf) / (tp - tf)) duty = scale_d * duty print("approach item(s)") duty_r = duty duty_l = duty print("Picking Up Item(s)") # drive function to drive forward for a few seconds # time.sleep(0.2) # call out encoders # resume random path print("Item has been picked up") rcpy.set_state(rcpy.EXITING) print("Resuming Search") # addition of recentering scuttle if lost for future else: # recenter scuttle if still in view field print("Still Picking Up Item(s)") case = "turning" duty_l = round((x - 0.5 * width) / (0.5 * width), 2) # Duty Left duty_l = duty_l * scale_t duty_r = round((0.5 * width - x) / (0.5 * width), 2) # Duty Right duty_r = duty_r * scale_t print("Resume Search") # resume random path cleaning # rcpy.set_state(rcpy.EXITING) # Keep duty cycle within range for right wheel if duty_r > 1: duty_r = 1 elif duty_r < -1: duty_r = -1 # Keep duty cycle within range for left wheel if duty_l > 1: duty_l = 1 elif duty_l < -1: duty_l = -1 # Round duty cycles duty_l = round(duty_l, 2) duty_r = round(duty_r, 2) # Set motor duty cycles motor.set(motor_l, duty_l) motor.set(motor_r, duty_r) elif rcpy.get_state() == rcpy.EXITING: pass
self.check_hold(self.DIRECTION_TURN_RIGHT) if self.duty_turn <= self.DUTY_TURN_MAX - self.DUTY_TURN_ITERATION_VALUE: # If the next iteration is at most one iteration from max duty... self.duty_turn = self.duty_turn + self.DUTY_TURN_ITERATION_VALUE # Iterate else: # Value is greater than one iteration away from max duty self.duty_turn = self.DUTY_TURN_MAX # Set duty to its max value else: # Neither or both directions selected self.check_hold(self.DIRECTION_TURN_CENTER ) # Set direction to straight and reset duty try: m = Manual() m.change_direction(m.DIRECTION_THROTTLE_BACKWARD) m.increase_throttle() while rcpy.get_state() != rcpy.EXITING: m.turning(m.LEFT) motor.set(1, m.get_duty_turn()) motor.set(2, m.get_duty_throttle()) time.sleep(.1) # sleep some except KeyboardInterrupt: m.kill() pass finally: print("\nBye BeagleBone!")
import rcpy import rcpy.motor as motor motor_r = 2 # Right Motor motor_l = 1 # Left Motor rcpy.set_state(rcpy.RUNNING) while rcpy.get_state() != rcpy.EXITING: if rcpy.get_state() == rcpy.RUNNING: duty_l = 1 duty_r = 1 motor.set(motor_l, duty_l) motor.set(motor_r, duty_r)
motor.set(motor_l, speed) def MotorR(speed): motor.set(motor_r, speed) def getDcJack(): # return the voltage measured at the barrel plug voltage = round(get_dc_jack_voltage(), 2) return (voltage) # # Uncomment this section to run this program as a standalone loop v = getDcJack() print("startv:", v) while ((rcpy.get_state() != rcpy.EXITING) and (v >= 7)): if rcpy.get_state() == rcpy.RUNNING: v = getDcJack() print(v) MotorL(-1) MotorR(1) time.sleep(30) v = getDcJack() print(v) MotorL(1) MotorR(-1) time.sleep(30) v = getDcJack() print(v)
srvo1 = servo.Servo(ch1) # Create servo object srvo2 = servo.Servo(ch2) clck1 = clock.Clock(srvo1, period) # Set PWM period for servos clck2 = clock.Clock(srvo2, period) servo.enable() # Enables 6v rail on beaglebone blue clck1.start() # Starts PWM clck2.start() def move1(angle): srvo1.set(angle) def move2(angle): srvo2.set(angle) # THE SECTION BELOW WILL ONLY RUN IF L1_SERVO.PY IS CALLED DIRECTLY if __name__ == "__main__": while True: print("beginning servo loop") while rcpy.get_state() != rcpy.EXITING: # keep running print("move 1.5") move1( 1.5 ) # Set servo duty (1.5 has no units, see library for details) time.sleep(2) print("move -1.5") move1(-1.5) time.sleep(2)
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "hmpcagtqfos:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_dmp: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults enable_magnetometer = False show_compass = False show_gyro = False show_accel = False show_quat = False show_tb = False sample_rate = 100 enable_fusion = False show_period = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-s": sample_rate = int(a) elif o == "-m": enable_magnetometer = True elif o == "-c": show_compass = True elif o == "-a": show_accel = True elif o == "-g": show_gyro = True elif o == "-q": show_quat = True elif o == "-t": show_tb = True elif o == "-f": enable_fusion = True elif o == "-p": show_period = True else: assert False, "Unhandled option" if show_compass and not enable_magnetometer: print('rcpy_test_dmp: -c can only be used with -m ') usage() sys.exit(2) # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # magnetometer ? mpu9250.initialize(enable_dmp = True, dmp_sample_rate = sample_rate, enable_fusion = enable_fusion, enable_magnetometer = enable_magnetometer) # message print("Press Ctrl-C to exit") # header if show_accel: print(" Accel XYZ (m/s^2) |", end='') if show_gyro: print(" Gyro XYZ (deg/s) |", end='') if show_compass: print(" Mag Field XYZ (uT) |", end='') print("Head(rad)|", end='') if show_quat: print(" Quaternion |", end='') if show_tb: print(" Tait Bryan (rad) |", end='') if show_period: print(" Ts (ms)", end='') print() try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: t0 = time.perf_counter() data = mpu9250.read() t1 = time.perf_counter() dt = t1 - t0 t0 = t1 print('\r', end='') if show_accel: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' .format(data['accel']), end='') if show_gyro: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} |' .format(data['gyro']), end='') if show_compass: print('{0[0]:7.1f} {0[1]:7.1f} {0[2]:7.1f} |' .format(data['mag']), end='') print(' {:6.2f} |' .format(data['head']), end='') if show_quat: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} {0[3]:6.1f} |' .format(data['quat']), end='') if show_tb: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' .format(data['tb']), end='') if show_period: print(' {:7.2f}'.format(1000*dt), end='') # no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")