def write(self, *values):
#print('> write to motor')
if self.enabled:
assert len(values) == 1
mtr.set(self.motor, values[0] / self.ratio)
def kill(self):
self.direction = 1
self.direction_turn = 0
self.duty_throttle = 0
self.duty_turn = 0
motor.set(1, 0)
motor.set(2, 0)
print("Killswitch activated.")
def set_enabled(self, enabled = True):
# call super
super().set_enabled(enabled)
if not enabled:
# write 0 to motor
mtr.set(self.motor, 0)
mtr.set_free_spin(self.motor)
def MotorR(speed): # takes argument in range [-1,1]
motor.set(motor_r, speed)
# Uncomment this section to run this program as a standalone loop
# 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("L1_motors.py: driving fwd")
# MotorL(0.6) # gentle speed for testing program. 0.3 PWM may not spin the wheels.
# MotorR(0.6)
# time.sleep(4) # run fwd for 4 seconds
def test1():
N = 5
try:
# enable
motor.enable()
time.sleep(.5)
# disable
motor.disable()
time.sleep(.5)
# enable again
motor.enable()
# spin motor 2 forward and back
motor.set(2, 1)
time.sleep(2)
motor.set_free_spin(2)
time.sleep(1)
motor.set(2, -1)
time.sleep(2)
motor.set_free_spin(2)
# spin motor 2 forward and back
motor.set(3, 1)
time.sleep(2)
motor.set_free_spin(3)
time.sleep(3)
motor.set(3, -1)
time.sleep(2)
motor.set_free_spin(3)
# disable motor
motor.disable()
except (KeyboardInterrupt, SystemExit):
pass
finally:
motor.disable()
def test1():
N = 5
try:
# enable
motor.enable()
time.sleep(.5)
# disable
motor.disable()
time.sleep(.5)
# enable again
motor.enable()
# spin motor 2 forward and back
motor.set(2, 1)
time.sleep(2)
motor.set_free_spin(2)
time.sleep(1)
motor.set(2, -1)
time.sleep(2)
motor.set_free_spin(2)
# spin motor 2 forward and back
motor.set(3, 1)
time.sleep(2)
motor.set_free_spin(3)
time.sleep(3)
motor.set(3, -1)
time.sleep(2)
motor.set_free_spin(3)
# disable motor
motor.disable()
except (KeyboardInterrupt, SystemExit):
pass
finally:
motor.disable()
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!")
def step(direction):
crane_bottom_switch_state = GPIO.input(crane_bottom_switch) #damn them long variable names
if direction == -1 and crane_bottom_switch_state == 0: # If endstop is pressed and trying
# to go down do not moe stepper.
direction = 0
# Check direction input value
# Change direction value if not betweeen 0 and 3
if direction > 0:
stepinfo.state += 1
elif direction < 0:
stepinfo.state -= 1
if stepinfo.state > 3:
stepinfo.state = 0
elif stepinfo.state < 0:
stepinfo.state = 3
# Send stepper coils power.
if stepinfo.state == 0:
motor.set(M1,1)
motor.set(M2,1)
elif stepinfo.state == 1:
motor.set(M1,-1)
motor.set(M2,1)
elif stepinfo.state == 2:
motor.set(M1,-1)
motor.set(M2,-1)
elif stepinfo.state == 3:
motor.set(M1,1)
motor.set(M2,-1)
else:
motor.set(M1,1)
motor.set(M2,1)
time.sleep(0.003) # 10ms delay
if duty_l < -1:
duty_l = -1
if duty_r < -1:
duty_r = -1
# Print for debugging
# print("Duty L: ",round(duty_l,2),"\t\tDuty R: ",round( duty_r,2), "\t\tLB: ", l_button, "\t\tRB: ", r_button)
# Set motor values
motor.set(motor_l, round(duty_l,2))
motor.set(motor_r, round(duty_r,2))
elif rcpy.get_state() == rcpy.PAUSED:
pass
signal.signal(signal.SIGINT, home_crane)
except KeyboardInterrupt: # condition added to catch a "Ctrl-C" event and exit cleanly
print("Exiting")
rcpy.set_state(rcpy.EXITING)
pass
#print("data 3", data[1])
dutyY = -1 * (int(data[1]) - 255) / 255
#print("dutyY", data[1])
elif int(data[2]) == 170:
#print("data 3", data[1])
dutyY = 1 * (int(data[1]) - 255) / 255
#print("dutyY", data[1])
motor.set(Motor_X, dutyX)
motor.set(Motor_Y, dutyY)
pass
# paused?
elif rcpy.get_state() == rcpy.PAUSED:
# do nothing
pass
except KeyboardInterrupt:
def main():
candistance = ultrasonic.distanceMeasurement(ultrasonic.trig_pin,ultrasonic.echo_pin)
if candistance < 5:
print("ready")
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:
x = 0
motor_r = 2 # Right Motor assigned to #2
motor_l = 1 # Left Motor assigned to #1
while x < 20
clo = vector.getNearest()
#Convert to angle and distance
u = math.sin(clo[1])
dis = clo[0] * abs(u)
v = math.cos(clo[1])
dis2 = clo[0] * abs(v)
dir = "driving"
if clo[1] < 0 and clo[1] >= -75 and dis < 0.2 and dis2 < 0.2:
print(dis)
motor.set(motor_l, -0.6)
motor.set(motor_r, 0.62)
time.sleep(2.75)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
motor.set(motor_l, 0.97)
motor.set(motor_r, 1)
time.sleep(0.5)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
motor.set(motor_l, 0.6)
motor.set(motor_r, -0.62)
time.sleep(2.75)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
duty_r = 0
duty_l = 0
#Case 2: obstruction the the right of the SCUTTLE
elif clo[1] > 0 and clo[1] <= 75 and dis < 0.15 and dis2 < 0.15 :
print(dis)
motor.set(motor_l, 0.6)
motor.set(motor_r, -0.62)
time.sleep(2.75)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
motor.set(motor_l, 0.97)
motor.set(motor_r, 1)
time.sleep(0.5)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
motor.set(motor_l, -0.6)
motor.set(motor_r, 0.62)
time.sleep(2.75)
motor.set(motor_l, 0)
motor.set(motor_r, 0)
time.sleep(0.5)
else
motor.set(motor_l, 0.97)
motor.set(motor_r, 1)
time.sleep(0.5)
motor.set
x = x+1
# start clock
servos.clck.start() # Starts PWM 1
servos.clck2.start() # Starts PWM 2
# keep running
servos.srvo.set(servos.duty) # Set motor 1 duty
servos.srvo2.set(servos.duty2) #set motor 2 duty
while(x<5750):
x = x + 1
print(x)
print("out of loop")
servos.srvo.set(servos.off)
servos.srvo2.set(servos.off)
print("off")
# stop clock
servos.clck.stop()
servos.clck2.stop()
rcpy.set_state(rcpy.EXITING)
pass
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
elif candistance > 5:
print("Error: No can")
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 MotorR(speed):
motor.set(motor_r, speed)
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 motors(x, y):
motor.set(motor_x, x)
motor.set(motor_y, y)
stage = 0
motorset = 5
motor_pwm = [
0, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35, 0.40, 0.45, 0.50, 0.55, 0.60,
0.65, 0.70, 0.75, 0.80, 0.85, 0.90, 0.95, 1
]
motor_spd = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
old_enc = 0
Ts = 1
for i in motor_pwm:
for j in range(0, 10):
if motorset != i:
print('New Stage Setted')
motorset = i
motor.set(i)
print(f'PWM = {i},Timers = {j}')
ne = encoder.get()
spd = (ne - old_enc) / Ts
old_enc = ne
time.sleep(Ts)
motor_spd[stage] = spd
print(f'stage = {stage}')
stage += 1
time.sleep(Ts)
print(motor_spd)
motor.free_spin()
with open('test.txt', 'w') as f:
f.write(json.dumps(motor_pwm))
f.write(json.dumps(motor_spd))
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")
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!")
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 set_speed(speedL,
speedR): #in one function, cmd both motor driver channels
motor.set(Motor_L, ((speedL - 127) / 127)) #h bridge commands
motor.set(Motor_R, ((speedR - 127) / 127))
KiTerm = Ki * einteg # generate Ki signal
KdTerm = Kd * de_dt # generate Kd signal
u1 = round((KpTerm + KdTerm + KiTerm),3 ) # control signal is combo of 3 terms * error
#Limit the change in u each loop
dev = 0.025 # permitted control deviation from one loop to next
if u1 > (u0+dev):
u1 = (u0+dev)
elif u1 < (u0 - dev):
u1 = (u0 - dev)
# Place Min/Max bounds on U
u = sorted([0, u1, 1])[1] #the function constrains a variable between 1st and 3rd args
u0 = u # store previous u
motor.set(Motor_L, u1)
iteration += 1
if iteration%10 == 1: #print info every 10 samples
print("x1: ", round(x1,4), "x0: ", round(x0,4), "dt: ", round(dt1,4), "v1: ",v1, "e1: ", e1,"einteg: ", einteg," u: ", round(u,4))
if iteration%300 == 0:
motor.set(Motor_L,0) #stop the motor
rcpy.set_state(rcpy.EXITING) #exit the main loop
time.sleep(period) #delay by one period
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)
def MotorL(speed):
motor.set(motor_l, speed)
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
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)
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)
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!")
def MotorR(speed): # takes argument in range [-1,1]
motor.set(motor_r, speed)
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:
# say bye
print("\nBye BeagleBone!")
def accy(state, channel): # takes argument in range [-1,1]
motor.set(channel, state)
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 = 0 # 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
# handle centered condition
if x > ((width/2)-(band/2)) and x < ((width/2)+(band/2)): # If center point is centered
# item centered to SCUTTLE center of usb camera "centered"
print("Item Centered")
# once SCUTTLE is center; drive towards metal items. "picking up"
dir = "driving"
case = "too far"
duty = 1 - ((radius - tf)/(tp - tf))
duty = scale_d * duty
duty_r = duty
duty_l = duty
time.sleep(0.2) # call out encoders
print("Picking Up Item(s)")
else: # recenter scuttle
print("ERROR: Re-Centering SCUTTLE")
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
#else: # item is not in the frame check Load cell
# check load cell reading to ensure "item has been picked"
# currentADC = round(load.,2) # round adc value readings which is dependent
# previousADC = round(load.,2) # round adc value readings which is dependent
# loadStatus = (previousADC - currentADC)
# if loadStatus > 0: # might need an incremental reading range
# print("Success: Item has been picked up")
# Exit L3_pickup
# rcpy.set_state(rcpy.EXITING)
# resume random path cleaning
# else:
# print("ERROR: Failed to pickup item")
# rcpy.set_state(rcpy.EXITING)
# resume random path cleaning
# 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.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.")
