def __init__(self):
self.direction = self.DIRECTION_THROTTLE_DEFAULT
self.direction_turn = self.DIRECTION_TURN_DEFAULT
self.duty_throttle = 0
self.duty_turn = 0
rcpy.set_state(rcpy.RUNNING)
print("Manual Initiated.")
def main(duty):
# # exit if no options
# if len(sys.argv) < 2:
# usage()
# sys.exit(2)
# # Parse command line
# try:
# opts, args = getopt.getopt(sys.argv[1:], "hst:d:c:", ["help"])
# except getopt.GetoptError as err:
# # print help information and exit:
# print('rcpy_test_servos: illegal option {}'.format(sys.argv[1:]))
# usage()
# sys.exit(2)
# defaults
# duty = 1.5
period = 0.02
channel = 0
sweep = False
brk = False
free = False
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(channel)
srvo.set(duty)
clck = clock.Clock(srvo, period)
servo.enable()
clck.start()
def __init__(self):
# Hips are servos 1 - 4 clockwise from the front right
# Ankles are servos 5 -8 clockwise from the front right
rcpy.set_state(rcpy.RUNNING)
self.servos = [servo.Servo(srv) for srv in range(1, 9)]
self.clocks = [clock.Clock(srv, .02) for srv in self.servos]
self.reset()
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 init_steering(init_data):
global steering_servo
# Setup steering servo
rcpy.set_state(rcpy.RUNNING)
servo.enable()
steering_servo = servo.Servo(init_data["channel"])
steering_servo.start(init_data["pwm_period"])
print("Steering initialized. YOU LOVELY PERSON!")
def setup():
GPIO.setup(pause, GPIO.IN)
bus.write_byte_data(matrix, 0x21, 0)
bus.write_byte_data(matrix, 0x81, 0)
bus.write_byte_data(matrix, 0xef, 0)
bus.write_i2c_block_data(matrix, 0, [0] * 16)
rcpy.set_state(rcpy.RUNNING)
def train():
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_magnetometer=False)
while True:
data = mpu9250.read()
print(data['gyro'][0], data['gyro'][1], data['gyro'][2])
time.sleep(0.1)
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 shutdown(self, quiet=False):
self.print_stdout("shutdown(quiet={})".format(quiet), quiet)
self.running = False
# stop clock
for i in range(0, 8):
self.clcks[i].stop()
# disable servos
servo.disable()
rcpy.set_state(rcpy.EXITING)
GPIO.cleanup()
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 __init__(self):
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# no magnetometer
mpu9250.initialize(
enable_dmp=True,
dmp_sample_rate=200,
enable_fusion=True,
enable_magnetometer=True
)
# start the sensor
rcpy.set_state(rcpy.RUNNING)
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 initialize_sensors():
print("\n--- Sensors initializing...")
try:
#For a raspberry pi, for example, to set up pins 4 and 5, you would add
#GPIO.setmode(GPIO.BCM)
#GPIO.setup(4, GPIO.IN)
#GPIO.setup(5, GPIO.IN)
# Set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# Activate the magnetometer on the BeagleBone Blue
rcpy.mpu9250.initialize(enable_magnetometer = True)
print("--- Sensors initialized!")
# In short, in this example, by default,
# this function is called but doesn't do anything (it's just a placeholder)
except Exception as ex:
# Log any error, if it occurs
print(str(datetime.datetime.now()) + " Error when initializing sensors: " + str(ex))
def home_crane(sig,frame):
print("Crane moving to home position...")
while GPIO.input(crane_bottom_switch) == 1:
step(-1)
signal.signal(signal.SIGINT, no_ctrl_c)
time.sleep(0.5)
print("\nCrane returned to home.")
rcpy.set_state(rcpy.EXITING)
pass
def __init__(self):
super().__init__()
self.running = False
self.reload()
period = self.period
self.bbb_servos = [
servo.Servo(1),
servo.Servo(2),
servo.Servo(3),
servo.Servo(4),
servo.Servo(5),
servo.Servo(6),
servo.Servo(7),
servo.Servo(8)
]
self.clcks = [
clock.Clock(self.bbb_servos[0], period),
clock.Clock(self.bbb_servos[1], period),
clock.Clock(self.bbb_servos[2], period),
clock.Clock(self.bbb_servos[3], period),
clock.Clock(self.bbb_servos[4], period),
clock.Clock(self.bbb_servos[5], period),
clock.Clock(self.bbb_servos[6], period),
clock.Clock(self.bbb_servos[7], period)
]
self.motors = [
motor.Motor(1),
motor.Motor(2),
motor.Motor(3),
motor.Motor(4)
]
# Boot
GPIO.cleanup()
rcpy.set_state(rcpy.RUNNING)
# disable servos
servo.enable()
# start clock
for i in range(0, 8):
self.clcks[i].start()
self.tts_engine = pyttsx3.init()
self.tts_engine.setProperty('rate', 150)
self.running = True
def __init__(self, rotations, frequency, channel, sweep=False, brk=False, free=False, update_interval=100):
self.duty = rotations*3/8 - 1.5
self.period = 1/frequency
self.channel = channel
self.sweep = sweep
self.brk = brk
self.free = free
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# set servo duty (only one option at a time)
self.internal = servo.Servo(self.channel)
self.clck = clock.Clock(self.internal, self.period)
self.delta = self.duty/100
self.timer = threading.Timer(update_interval)
def PushLights():
# Get rcpy running
rcpy.set_state(rcpy.RUNNING)
# Welcome message
print("=" * 65)
print("Welcome to PressyLights!")
print(
"Press <PAUSE> to turn on the red light, or <MODE> to turn on the green light."
)
print("Press both <PAUSE> and <MODE> to exit.")
print("-" * 65)
# Main loop
while not (mode.is_pressed() and pause.is_pressed()):
if pause.is_pressed():
red.on()
green.off()
elif mode.is_pressed():
red.off()
green.on()
# Exit message
print("Bye Beaglebone!")
print("=" * 65)
red.off()
green.off()
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 set_angle(self, angle):
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(self.channel)
duty = self.angle_to_duty(angle)
print(duty)
srvo.set(duty)
clck = clock.Clock(srvo, self.period)
servo.enable()
clck.start()
clck.stop()
servo.disable()
self.duty = duty
msg = servolcm()
msg.duty = self.duty
msg.timestamp = datetime.strftime(datetime.now(), datetime_format)
self.lc.publish("servo_data", msg.encode())
def setup(self):
# # Set the two LEDs as outputs:
# GPIO.setup(self.inputpin, GPIO.OUT)
# GPIO.setup(self.inputpin, GPIO.OUT)
#
# # Start one high and one low:
# GPIO.output(self.inputpin, GPIO.HIGH)
#GPIO.output("SERVO_PWR", GPIO.HIGH)
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
self.rcpy_state = rcpy.RUNNING
# set servo duty (only one option at a time)
#Enable Servos
servo.enable()
self.srvo = servo.Servo(self.num)
if self.duty != 0:
print('Setting servo {} to {} duty'.format(self.num, self.duty))
self.srvo.set(self.duty)
self.currentState = True
self.clk = clock.Clock(self.srvo, self.period)
# start clock
self.clk.start()
def __init__(self):
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# front esc
self.front_right = servo.ESC(esc_const.EscPwmPins.FRONT_RIGHT)
self.front_left = servo.ESC(esc_const.EscPwmPins.FRONT_LEFT)
# back esc
self.back_right = servo.ESC(esc_const.EscPwmPins.BACK_RIGHT)
self.back_left = servo.ESC(esc_const.EscPwmPins.BACK_LEFT)
self.escs = [
self.front_right, self.back_left, self.front_left, self.back_right
]
self.logger = logging.getLogger('MotorController')
self.logger.debug('__init__')
# create clock to do periodic updates
self.fr_clock = clock.Clock(self.front_right, self.UPDATE_PERIOD)
self.fl_clock = clock.Clock(self.front_left, self.UPDATE_PERIOD)
self.br_clock = clock.Clock(self.back_right, self.UPDATE_PERIOD)
self.bl_clock = clock.Clock(self.back_left, self.UPDATE_PERIOD)
# set all to zero
self.front_right.set(0)
self.front_left.set(0)
self.back_right.set(0)
self.back_left.set(0)
# start all the clocks
self.fr_clock.start()
self.fl_clock.start()
self.br_clock.start()
self.bl_clock.start()
# This example drives the right and left motors.
# Intended for Beaglebone Blue hardware.
# This example uses rcpy library. Documentation: guitar.ucsd.edu/rcpy/rcpy.pdf
# Import external libraries
import rcpy
import rcpy.motor as motor
import time # only necessary if running this program as a loop
import numpy # for clip function
motor_l = 1 # Left Motor (ch1)
motor_r = 2 # Right Motor (ch2)
# NOTE: THERE ARE 4 OUTPUTS. 3 & 4 ACCESSIBLE THROUGH diode & accy functions
rcpy.set_state(rcpy.RUNNING) # initialize the rcpy library
# define functions to command motors, effectively controlling PWM
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)
# Uncomment this section to run this program as a standalone loop
def turn(angle=None, mode=None, speed=30, direction=None):
turn = True
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_dmp=True)
while turn:
if rcpy.state() == rcpy.RUNNING:
imu_data = mpu9250.read() # Read Data from Sensors
imu = imu_data[
'tb'] # Get imu Value and Convert to Degrees (0 to 180 , -180 to 0)
imu_deg = (math.degrees(round(imu[2], 2))) % 360
angle = angle % 360
if mode == "point" or mode == None:
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128 - speed] # Rotate Left
data_r = [255, 1, 128 + speed]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128 + speed] # Rotate Right
data_r = [255, 1, 128 - speed]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
elif mode == "swing":
if direction == "forward":
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128] # Rotate Left
data_r = [255, 1, 128 + speed]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128 + speed] # Rotate Right
data_r = [255, 1, 128]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
elif direction == "backward":
if angle == 0:
data_l = [146, 32] # Brake
data_r = [146, 32]
# Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn
elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise
data_l = [255, 0, 128 - speed] # Rotate Left
data_r = [255, 1, 128]
elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise
data_l = [255, 0, 128] # Rotate Right
data_r = [255, 1, 128 - speed]
if (imu_deg - 2) <= angle and angle <= (
imu_deg + 2): # Once Angle is within Range Stop Motors
data_l = [146, 32] # Brake
data_r = [146, 32]
else:
continue
ser_motor.write(data_l) # Send Data to Motor Controllers
ser_motor.write(data_r)
data_l = [146, 32] # Brake
data_r = [146, 32]
ser_motor.write(data_l) # Send Data to Motor Controllers
ser_motor.write(data_r)
turn = False
print("Done!")
pass
datalen = len(data['poses'])
for i in range(0, datalen):
print(i, data['poses'][i]['position']['x'],
data['poses'][i]['position']['z'])
srvo[i].set(data['poses'][i]['position']['z'])
#print('open finish')
time.sleep(1)
return
initial()
while (True):
servo.enable()
print("Receiving...\n")
rcpy.set_state(rcpy.RUNNING)
result = ws.recv()
print("\n" + result + "\n")
mm = json.loads(result)
print(mm['msg'])
print(mm['msg']['goal']['order'])
revnum = mm['msg']['goal']['order']
for i in range(3):
object = ['button', 'bottle', 'phone']
print(object[revnum])
jsname = object[revnum]
jsfile = str(jsname) + ".json"
print(jsfile)
time.sleep(1)
action()
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!")
# PROGRAM REQUIRES SUDO. Last udpated 2020.10.08
# Import external libraries
import time, math
import getopt, sys
import rcpy # This automatically initizalizes the robotics cape
import rcpy.servo as servo
import rcpy.clock as clock # For PWM period for servos
# INITIALIZE DEFAULT VARS
duty = 1.5 # Duty cycle (-1.5,1.5 is the typical range)
period = 0.02 # recommended servo period: 20ms (this is the interval of commands)
ch1 = 1 # select channel (1 thru 8 are available)
ch2 = 2 # selection of 0 performs output on all channels
rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING
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):
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
def start(self):
rcpy.set_state(rcpy.RUNNING)
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
# 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
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()
def Exit():
rcpy.set_state(rcpy.EXITING)
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 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!")
#!/usr/bin/env python3
# Run the motors
# Based on: https://github.com/mcdeoliveira/rcpy/raw/master/examples/rcpy_test_motors.py
# import python libraries
import time
import rcpy
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
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!")
