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:
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!")
