def main():
global tp
try:
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
tp = TiltPan(egpg)
# #### SET CNTL-C HANDLER
myPyLib.set_cntl_c_handler(handle_ctlc)
while True:
print("tiltpan centered")
tp.tiltpan_center()
sleep(5)
print("pan(45)")
tp.pan(45)
sleep(5)
print("pan(135)")
tp.pan(135)
sleep(5)
print("pan(PAN_CENTER)")
tp.pan(PAN_CENTER)
print("tilt(45)")
tp.tilt(45)
sleep(5)
print("tilt(-45)")
tp.tilt(-45)
sleep(5)
print("tilt(TILT_CENTER)")
tp.tilt(TILT_CENTER)
tp.tiltpan_center()
print("pan_position:", tp.get_pan_pos())
print('UP too far')
tp.tilt(90)
print("tilt_position:", tp.get_tilt_pos())
sleep(5)
print('DOWN too far')
tp.tilt(-90)
print("tilt_position:", tp.get_tilt_pos())
sleep(5)
print("YES")
tp.nod_yes()
sleep(2)
print("NO")
tp.nod_no()
sleep(2)
print("IDK")
tp.nod_IDK()
sleep(2)
print("Turning tiltpan servos off")
tp.off()
sleep(5)
print("tilt_position(999=UNKNOWN):", tp.get_tilt_pos())
print("pan_position (999=UNKNOWN):", tp.get_pan_pos())
#end while
except SystemExit:
print("tiltpan.py: exiting")
def __init__(self):
# motor setup
self.gpg = easy.EasyGoPiGo3() # GoPiGo3 Motor
self.gpg.set_robot_constants(WHEEL_RADIUS * 2 * 1000,
WHEEL_DISTANCE * 1000)
rospy.Subscriber("cmd_vel", Twist, self.callback,
queue_size=1) # subscribe to cmd_vel topic
rospy.Subscriber("gopigo_vel", Gopigo, self.callback_gpg,
queue_size=1) # subscribe to cmd_vel topic
rospy.loginfo("Initialized controller class")
def main():
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
myconfig.setParameters(egpg)
tp = tiltpan.TiltPan(egpg)
tp.tiltpan_center()
print("tiltpan centered")
sleep(0.2)
tp.off()
print("tiltpan off")
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
myconfig.setParameters(egpg) # configure custom wheel dia and base
tp = tiltpan.TiltPan(egpg) # init tiltpan
print("centering")
tp.tiltpan_center()
print("centered")
sleep(0.5)
tp.off()
print("tiltpan.off() complete")
try:
# loop
loopSleep = 10 # second
loopCount = 0
keepLooping = True
while keepLooping:
loopCount += 1
# orbit_in_reverse(egpg, degrees= 45, radius_cm= egpg.WHEEL_BASE_WIDTH/2.0, blocking=True)
# exit(0)
print("\nTranslate 1 to 4 mm")
for dist_mm in range(1, 5, 1): # 1, 2, 3, 4 mm
print("\nTranslateMm({:.0f}) {:.2f} inches".format(
dist_mm, dist_mm / 25.4))
translateMm(egpg, dist_mm, debug=True) # to the left
sleep(5)
print("\nTranslateMm({:.0f}) {:.2f} inches".format(
-dist_mm, -dist_mm / 25.4))
translateMm(egpg, -dist_mm) # to the right
sleep(5)
print("\nTranslate 3, 2, 1 inches")
for dist_10x_mm in range(762, 0, -254): # 3, 2, 1 inch
dist_mm = dist_10x_mm / 10.0
print("\nTranslateMm({:.0f}) {:.0f} inches".format(
dist_mm, dist_mm / 25.4))
translateMm(egpg, dist_mm, debug=True) # to the left
sleep(5)
print("\nTranslateMm({:.0f}) {:.0f} inches".format(
-dist_mm, -dist_mm / 25.4))
translateMm(egpg, -dist_mm) # to the right
sleep(5)
keepLooping = False
#sleep(loopSleep)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def __init__(self): ## TODO: some of this should probably go in startup()?
self.api_full = gopigo3.GoPiGo3(
) # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
self.api_easy = easygopigo3.EasyGoPiGo3()
self.active = False
self.distance = self.api_easy.init_distance_sensor()
self.distance_servo = self.api_easy.init_servo("SERVO1")
self.camera = picamera.PiCamera()
self.camera_servo = self.api_easy.init_servo("SERVO2")
self.imu = inertial_measurement_unit.InertialMeasurementUnit(
bus="GPG3_AD1")
def main():
lifeLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
# loop
loopSleep = 1 # second
loopCount = 0
keepLooping = True
while keepLooping:
loopCount += 1
image = cv2.imread(args["image"])
print("width: {} pixels".format(image.shape[1]))
print("height: {} pixels".format(image.shape[0]))
cv2.imshow("Image", image)
# myimutils.display("Image", image, scale_percent=30)
(b, g, r,) = image[0,0]
print("Pixel at (0,0) - Red: {}, Green: {}, Blue: {}".format(r,g,b))
(b, g, r,) = image[219,90]
print("Pixel at (x:90,y:219) - Red: {}, Green: {}, Blue: {}".format(r,g,b))
print("Setting Pixel at (0,0) to Red")
image[0,0] = (0, 0, 255)
(b, g, r,) = image[0,0]
print("Pixel at (0,0) - Red: {}, Green: {}, Blue: {}".format(r,g,b))
corner = image[0:100, 0:100]
cv2.imshow("Corner", corner)
image[0:100, 0:100] = (0, 255, 0)
cv2.imshow("Updated", image)
cv2.waitKey(0)
# cv2.imwrite("newimage.jpg", image)
keepLooping = False
sleep(loopSleep)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
lifeLog.logger.info("Finished")
sleep(1)
def main():
if Carl: lifeLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
myconfig.setParameters(egpg)
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
keepLooping = True
while keepLooping:
# Create memory stream
stream = io.BytesIO()
# capture a frame from camera to the streem
with picamera.PiCamera() as camera:
# camera.resolution = (320, 240)
camera.resolution = (640, 480)
camera.capture(stream, format='jpeg')
frame_time = datetime.datetime.now().strftime("%H:%M:%S.%f")[:12]
# convert pic into numpy array
buff = np.fromstring(stream.getvalue(), dtype=np.uint8)
# create an OpenCV image
image = cv2.imdecode(buff, 1)
#myimutils.display("input",image)
#cv2.waitKey(0)
face_set = detect_faces(image)
# myimutils.display("faces()", face_set[0],50 ) # 640x480 images
cv2.imshow("faces()", face_set[0])
print("{}: faces() found {} faces".format(frame_time,
len(face_set[1])))
for (x, y, w, h) in face_set[1]:
print(" {}x{} face at ({},{})".format(h, w, x, y))
# wait time in ms: 500 to limit load, 1 to go as fast as possible (1 detect / sec)
k = cv2.waitKey(1)
if (k == 27):
keepLooping = False
cv2.destroyAllWindows()
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: lifeLog.logger.info("Finished")
sleep(1)
def main(args):
gopigo3 = easy.EasyGoPiGo3()
servo1 = gopigo3.init_servo("SERVO1")
servo2 = gopigo3.init_servo("SERVO2")
servo1_position = 86
servo1.rotate_servo(servo1_position)
servo2_position = 90
servo2.rotate_servo(servo2_position)
return 0
def main():
if Carl: runLog.logger.info("Started")
try:
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
except:
strToLog = "Could not instantiate an EasyGoPiGo3"
print(strToLog)
if Carl: lifeLog.logger.info(strToLog)
exit(1)
if Carl:
myconfig.setParameters(egpg)
tp = tiltpan.TiltPan(egpg)
tp.tiltpan_center()
tp.off()
try:
# Do Somthing in a Loop
loopSleep = 1 # second
loopCount = 0
keepLooping = False
while keepLooping:
loopCount += 1
# do something
sleep(loopSleep)
# Do Something Once
# load the image and convert to grayscale
image = cv2.imread(args["image"])
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# initialize the list of threshold methods
methods = [("THRESH_BINARY", cv2.THRESH_BINARY),
("THRESH_BINARY_INV", cv2.THRESH_BINARY_INV),
("THRESH_TRUNC", cv2.THRESH_TRUNC),
("THRESH_TOZERO", cv2.THRESH_TOZERO),
("THRESH_TOZERO_INV", cv2.THRESH_TOZERO_INV)]
# loop over the threshold methods
for (threshName, threshMethod) in methods:
# threshold the image and show it
(T, thresh) = cv2.threshold(gray, args["threshold"], 255,
threshMethod)
cv2.imshow(threshName, thresh)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def __init__(self, distance_hz=0):
self.gpg = easy.EasyGoPiGo3()
self.distance_sensor = self.gpg.init_distance_sensor()
self.servo = self.gpg.init_servo("SERVO2")
self.distance_hz = distance_hz
self.current_distance = mp.Value('i', 0)
self.current_direction = 'N'
self.backtracking = False
self.genList = list()
self.visited = list()
self.x = 0
self.y = 0
self.calibrationCount=0
def __init__(self):
self.gopigo3 = easy.EasyGoPiGo3()
self.keybindings = {
"w": ["Move the GoPiGo3 forward", "forward"],
"<SPACE>": ["Stop the GoPiGo3 from moving", "stop"],
"<UP>": [
"Take a distance sensor reading then adjust motor speed",
"read_respond_sensor"
],
"<DOWN>": ["Take a distance sensor reading", "test_sensor"],
"<ESC>": ["Exit", "exit"],
}
self.order_of_keys = ["w", "<SPACE>", "<UP>", "<DOWN>", "<ESC>"]
def __init__(self):
if not hasattr(self, "api_easy") or self.api_easy is None:
print("GSUbot.Robot __init__: initializing")
self.api_easy = easygopigo3.EasyGoPiGo3()
self.hardware = self.Hardware(self.api_easy)
self.firmware = self.Firmware(self.api_easy)
self.vitals = self.Vitals(self.api_easy)
self.distance = self.Distance(self.api_easy,
self.Servo(self.api_easy, "SERVO1"))
self.camera = self.Camera(self.Servo(self.api_easy, "SERVO2"))
self.imu = self.IMU(self.api_easy)
print("GSUbot.Robot __init__: initialized")
else:
print("GSUbot.Robot __init__: already initialized")
def main():
if Carl: runLog.logger.info("Started")
try:
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
except:
strToLog = "Could not instantiate an EasyGoPiGo3"
print(strToLog)
if Carl: lifeLog.logger.info(strToLog)
exit(1)
if Carl:
myconfig.setParameters(egpg)
tp = tiltpan.TiltPan(egpg)
tp.tiltpan_center()
tp.off()
try:
# Do Somthing in a Loop
loopSleep = 1 # second
loopCount = 0
keepLooping = False
while keepLooping:
loopCount += 1
# do something
sleep(loopSleep)
# Do Something Once
# load the image and convert to grayscale
image = cv2.imread(args["image"])
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(image, (5, 5), 0)
cv2.imshow("Image", image)
thresh = cv2.adaptiveThreshold(blurred, 255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY_INV, 11, 4)
cv2.imshow("Mean Thresh", thresh)
thresh = cv2.adaptiveThreshold(blurred, 255,
cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY_INV, 15, 3)
cv2.imshow("Mean Thresh", thresh)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
myconfig.setParameters(egpg)
tp = tiltpan.TiltPan(egpg)
tp.tiltpan_center()
tp.off()
try:
image = cv2.imread(args["image"])
cv2.imshow("Original", image)
blurred = np.hstack([
cv2.blur(image, (3, 3)),
cv2.blur(image, (5, 5)),
cv2.blur(image, (7, 7))
])
cv2.imshow("Averaged", blurred)
blurred2 = np.hstack([
cv2.GaussianBlur(image, (3, 3), 0),
cv2.GaussianBlur(image, (5, 5), 0),
cv2.GaussianBlur(image, (7, 7), 0)
])
cv2.imshow("Gaussian", blurred2)
blurred3 = np.hstack([
cv2.medianBlur(image, 3),
cv2.medianBlur(image, 5),
cv2.medianBlur(image, 7)
])
cv2.imshow("Median", blurred3)
blurred4 = np.hstack([
cv2.bilateralFilter(image, 5, 21, 21),
cv2.bilateralFilter(image, 7, 31, 31),
cv2.bilateralFilter(image, 9, 41, 41)
])
cv2.imshow("Bilateral", blurred4)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def __init__(self):
self.pi = easygopigo3.EasyGoPiGo3()
self.pi.set_speed(100)
self.width = 640
self.height = 480
self.camera = picamera.PiCamera(resolution=(self.width, self.height),
framerate=10)
self.camera.iso = 200
# Wait for the automatic gain control to settle
time.sleep(2)
# Fix the values
self.camera.shutter_speed = self.camera.exposure_speed
self.camera.exposure_mode = 'off'
g = self.camera.awb_gains
self.camera.awb_mode = 'off'
self.camera.awb_gains = g
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
image = cv2.imread(args["image"])
cv2.imshow("Original", image)
# aspect ratio = new width over orig width
r = 150 / image.shape[1]
# new dimensions will be (new width, orig_height * aspect ratio)
dim = (150, int(image.shape[0] * r))
resized = cv2.resize(image, dim, interpolation=cv2.INTER_AREA)
cv2.imshow("Resized Width", resized)
# aspect ratio = new height over orig height
r = 50.0 / image.shape[0]
# new dimesion = adjust width by aspect ratio
dim = (int(image.shape[1] * r), 50)
# Adrian says INTER_AREA is best resize interpolation type, lets try cubic
resized = cv2.resize(image, dim, interpolation=cv2.INTER_CUBIC)
cv2.imshow("Resized Height w/cubic", resized)
resized = imutils.resize(image, width=100)
cv2.imshow("imutils.resize width", resized)
resized = imutils.resize(image, height=50)
cv2.imshow("imutils.resize height w/area interp", resized)
resized = imutils.resize(image, width=120, height=60)
cv2.imshow("imutils.resize w120 h60", resized)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def __init__(self):
"""
Instantiates the key-bindings between the GoPiGo3 and the keyboard's keys.
Sets the order of the keys in the menu.
"""
self.gopigo3 = easy.EasyGoPiGo3()
self.servo1 = self.gopigo3.init_servo("SERVO1")
self.servo2 = self.gopigo3.init_servo("SERVO2")
self.keybindings = {
"<F1>":
["Turn SERVO1 completely to 0 degrees", "leftservo1_immediately"],
"<F2>": [
"Turn SERVO1 completely to 180 degrees",
"rightservo1_immediately"
],
"<F5>":
["Turn SERVO2 completely to 0 degrees", "leftservo2_immediately"],
"<F6>": [
"Turn SERVO2 completely to 180 degrees",
"rightservo2_immediately"
],
"a": [
"Turn SERVO1 towards 0 degrees incrementely",
"leftservo1_incrementally"
],
"d": [
"Turn SERVO1 towords 180 degrees incrementely",
"rightservo1_incrementally"
],
"<LEFT>": [
"Turn SERVO2 towards 0 degrees incrementely",
"leftservo2_incrementally"
],
"<RIGHT>": [
"Turn SERVO2 towards 180 degrees incrementely",
"rightservo2_incrementally"
],
"<SPACE>": ["Turn off power supply to both servos", "kill"],
"<ESC>": ["Exit", "exit"],
}
self.order_of_keys = [
"<F1>", "<F2>", "<F5>", "<F6>", "a", "d", "<LEFT>", "<RIGHT>",
"<SPACE>", "<ESC>"
]
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
#image = cv2.imread(args["image"])
#cv2.imshow("Original", image)
rectangle = np.zeros((300, 300), dtype = "uint8")
cv2.rectangle( rectangle, (25,25), (275,275), 255, -1)
cv2.imshow("Rectangle", rectangle)
circle = np.zeros((300,300), dtype = "uint8")
cv2.circle(circle, (150,150), 150, 255, -1)
cv2.imshow("Circle", circle)
bitwiseAnd = cv2.bitwise_and(rectangle, circle)
cv2.imshow("AND", bitwiseAnd)
bitwiseOr = cv2.bitwise_or(rectangle,circle)
cv2.imshow("OR", bitwiseOr)
bitwiseXor = cv2.bitwise_xor(rectangle, circle)
cv2.imshow("XOR", bitwiseXor)
bitwiseNot = cv2.bitwise_not(circle)
cv2.imshow("NOT", bitwiseNot)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def move():
gpg = easy.EasyGoPiGo3()
walld_distance_sensor = gpg.init_distance_sensor()
if randint(0, 1) == 0:
gpg.right()
else:
gpg.left()
time.sleep(1)
gpg.forward()
while True:
print("Distance Sensor Reading (mm): " +
str(walld_distance_sensor.read_mm()))
if walld_distance_sensor.read_mm() <= 50:
print("Stopping, Collision Detected")
gpg.stop()
break
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
image = cv2.imread(args["image"])
#cv2.imshow("Original", image)
(B, G, R) = cv2.split(image)
cv2.imshow("Red", R)
cv2.imshow("Green", G)
cv2.imshow("Blue", B)
merged = cv2.merge([B, G, R])
cv2.imshow("Merged", merged)
cv2.waitKey(0)
cv2.destroyAllWindows()
zeros = np.zeros(image.shape[:2], dtype = "uint8")
cv2.imshow("Red", cv2.merge([zeros, zeros, R]))
cv2.imshow("Green", cv2.merge([zeros, G, zeros]))
cv2.imshow("Blue", cv2.merge([B, zeros, zeros]))
cv2.waitKey(0)
cv2.destroyAllWindows()
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
image = cv2.imread(args["image"])
cv2.imshow("Original", image)
print("max of 255: {}".format(cv2.add(np.uint8([200]), np.uint8([100]))))
print("min of 0: {}".format(cv2.subtract(np.uint8([50]), np.uint8([100]))))
print("wrap around: {}".format(np.uint8([200]) + np.uint8([100])))
print("wrap around: {}".format(np.uint8([50]) - np.uint8([100])))
M = np.ones(image.shape, dtype = "uint8") * 100
added = cv2.add(image, M)
cv2.imshow("Added", added)
M = np.ones(image.shape, dtype = "uint8") * 50
subtracted = cv2.subtract(image, M)
cv2.imshow("Subtracted", subtracted)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
def __init__(self):
self.gopigo3 = easy.EasyGoPiGo3()
self.servo1 = self.gopigo3.init_servo("SERVO1")
self.keybindings = {
"w": ["Drive the GoPiGo along the delivery route", "delivery"],
"1": ["BRONZE Tier Challenge Function", "bronze"],
"2": ["SILVER Tier Challenge Function", "silver"],
"3": ["GOLD Tier Challenge Function", "gold"],
"<SPACE>": ["Stop the GoPiGo3 from moving", "stop"],
"<LEFT>":
["Turn servo completely to 0 degrees", "servo_total_left"],
"<UP>":
["Turn servo to 90 degrees (centered)", "servo_total_center"],
"<RIGHT>":
["Turn servo completely to 180 degrees", "servo_total_right"],
"<DOWN>": ["Take a distance sensor reading", "test_sensor"],
"<ESC>": ["Exit", "exit"],
}
self.order_of_keys = [
"w", "1", "2", "3", "<SPACE>", "<LEFT>", "<UP>", "<RIGHT>",
"<DOWN>", "<ESC>"
]
def __init__(self):
"""
This constructor sets the variables to the following values:
: CONST_GRIPPER_FULL_OPEN : Position of gripper servo when open
: CONST_GRIPPER_FULL_CLOSE: Position of gripper servo when closed
: CONST_GRIPPER_FULL_OPEN : Position of gripper servo to grab ball
: easygopigo3.EasyGoPiGo3 Easy_GPG: Initialization of EasyGoPiGo3
: easygopigo3 Easy_GPG: Initialization of EasyGoPiGo3
: easygopigo3.Servo gpgGripper: Initialization of Gripper Servo on Servo Pin 1
: init_distance_sensor my_distance_sensor: Initialization of Distance Sensor
: IOError: When the GoPiGo3 is not detected. It also debugs a message in the terminal.
: gopigo3.FirmwareVersionError: If the GoPiGo3 firmware needs to be updated. It also debugs a message in the terminal.
: Exception: For any other kind of exceptions.
"""
# GoPiGo Color Codes
self.YELLOW = (255, 255, 0)
self.GREEN = (0, 255, 0)
self.RED = (255, 0, 0)
self.BLUE = (0, 0, 255)
# Settings for cars in US Reston Office (these grippers were built differently)
self.CONST_GRIPPER_FULL_OPEN = 90
self.CONST_GRIPPER_FULL_CLOSE = 0
self.CONST_GRIPPER_GRAB_POSITION = 40
# Settings for cars in London Office (default method of assembly for grippers)
#self.CONST_GRIPPER_FULL_OPEN = 180
#self.CONST_GRIPPER_FULL_CLOSE = 20
#self.CONST_GRIPPER_GRAB_POSITION = 120
self.Easy_GPG = easygopigo3.EasyGoPiGo3(
) # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
self.gpgGripper1 = easygopigo3.Servo("SERVO1", self.Easy_GPG)
self.gpgGripper2 = easygopigo3.Servo("SERVO2", self.Easy_GPG)
self.my_distance_sensor = self.Easy_GPG.init_distance_sensor()
self.SetCarStatusLED(self.GREEN)
def main():
egpg = easygopigo3.EasyGoPiGo3(
use_mutex=True) # Create an instance of the EasyGoPiGo3 class
myconfig.setParameters(egpg)
runLog.logger.info("Starting scan360.py at {0:0.2f}v".format(egpg.volt()))
ds = myDistSensor.init(egpg)
tp = tiltpan.TiltPan(egpg)
# Adjust GOPIGO3 CONSTANTS to my bot default EasyGoPiGo3.WHEEL_DIAMETER = 66.5 mm
# egpg.WHEEL_DIAMETER = 59.0 # empirical from systests/wheelDiaRotateTest.py
# egpg.WHEEL_CIRCUMFERENCE = egpg.WHEEL_DIAMETER * math.pi
dist_list_mm = []
at_angle_list = []
# speeds = [300, 100, 50, 30]
speeds = [150, 50, 300]
for spd in speeds:
try:
print("\nSPIN 360 AND SCAN at speed={}".format(spd))
dist_list_mm, at_angle_list = spin_and_scan(
egpg, ds, tp, 360,
speed=spd) # spin in place and take distance sensor readings
range_list_cm = [dist / 10 for dist in dist_list_mm]
printmaps.view360(
range_list_cm,
at_angle_list) # print view (all r positive, theta 0=left
print("Readings:{}".format(len(at_angle_list)))
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
egpg.stop() # stop motors
runLog.logger.info("Exiting scan360.py at {0:0.2f}v".format(
egpg.volt()))
print("Ctrl-C detected - Finishing up")
egpg.stop()
def main():
if Carl: runLog.logger.info("Started")
egpg = easygopigo3.EasyGoPiGo3(use_mutex=True)
if Carl:
tiltpan.tiltpan_center()
sleep(0.5)
tiltpan.off()
try:
image = cv2.imread(args["image"])
cv2.imshow("Original", image)
# Remember - np image arrays are [y,x]
cropped = image[30:120, 240:335]
cv2.imshow("T-Rex Face", cropped)
cv2.waitKey(0)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
if (egpg != None): egpg.stop() # stop motors
print("\n*** Ctrl-C detected - Finishing up")
sleep(1)
if (egpg != None): egpg.stop()
if Carl: runLog.logger.info("Finished")
sleep(1)
#!/usr/bin/env python3 # -*- coding: utf-8 -*- # Import the simple GoPiGo3 module import easygopigo3 as go # Import time import time #Create an instance of the robot with a constructor from the easygopigo3 module that was imported as "go". myRobot = go.EasyGoPiGo3() # Set speed for the GoPiGo robot in degrees per second myRobot.set_speed(500) # Go forward myRobot.forward() # Block here for 1 second time.sleep(1) # Start turning right myRobot.right() # Keep turning right for 0.5 seconds time.sleep(0.5) # Start moving backwards myRobot.backward() # Block here for 1 seconds while the robot moves backwards time.sleep(1) # Stop the robot myRobot.stop()
import easygopigo3
import time
def change_color(color):
pi.set_eye_color(color)
pi.open_eyes()
time.sleep(5)
pi = easygopigo3.EasyGoPiGo3()
white = (255,255,255) #8bit RGB value
change_color(white)
pi.close_eyes()
def robotControl(trigger, simultaneous_launcher, motor_command_queue,
sensor_queue):
"""
Thread-launched function for orientating the robot around. It gets commands from the keyboard as well
as data from the sensor through the sensor_queue queue.
:param trigger: CTRL-C event. When it's set, it means CTRL-C was pressed and the thread needs to stop.
:param simultaneous_launcher: It's a barrier used for synchronizing all threads together.
:param motor_command_queue: Queue containing commands from the keyboard. The commands are read from within main.
:param sensor_queue: Processed data off of the IMU. The queue is intended to be read.
:return: Nothing.
"""
time_to_wait_in_queue = 0.1 # measured in
# try to connect to the GoPiGo3
try:
gopigo3_robot = EasyGoPiGo3()
except IOError:
print("GoPiGo3 robot not detected")
simultaneous_launcher.abort()
except gopigo3.FirmwareVersionError:
print("GoPiGo3 board needs to be updated")
simultaneous_launcher.abort()
except Exception:
print("Unknown error occurred while instantiating GoPiGo3")
simultaneous_launcher.abort()
# synchronizing point between all threads
# if abort method was called, then the synch will fail
try:
simultaneous_launcher.wait()
except threading.BrokenBarrierError as msg:
print("[robotControl] thread couldn't be launched")
# if threads were successfully synchronized
# then set the GoPiGo3 appropriately
if not simultaneous_launcher.broken:
gopigo3_robot.stop()
gopigo3_robot.set_speed(MOTORS_SPEED)
direction_degrees = None
move = False
acceptable_error_percent = 8
command = "stop"
rotational_factor = 0.30
accepted_minimum_by_drivers = 6
gpg = easy.EasyGoPiGo3()
my_distance_sensor = gpg.init_distance_sensor()
# while CTRL-C is not pressed, the synchronization between threads didn't fail and while the batteries' voltage isn't too low
while not (trigger.is_set() or simultaneous_launcher.broken
or gopigo3_robot.volt() <= MINIMUM_VOLTAGE):
# read from the queue of the keyboard
try:
command = motor_command_queue.get(timeout=time_to_wait_in_queue)
motor_command_queue.task_done()
except queue.Empty:
pass
# make some selection depending on what every command represents
if command == "stop":
move = False
elif command == "move":
move = True
if command == "west":
direction_degrees = -90.0
elif command == "east":
direction_degrees = 90.0
elif command == "north":
direction_degrees = 0.0
elif command == "south":
direction_degrees = 180.0
# if a valid orientation was selected
if direction_degrees is not None:
# read data and calculate orientation
heading = sensor_queue.get()
if direction_degrees == 180.0:
heading_diff = (direction_degrees -
abs(heading)) * (-1 if heading < 0 else 1)
error = abs(heading_diff / direction_degrees) * 100
else:
heading_diff = direction_degrees - heading
error = abs(heading_diff / 180) * 100
how_much_to_rotate = int(heading_diff * rotational_factor)
if DEBUG is True:
print(
"direction_degrees {} heading {} error {} heading_diff {}".
format(direction_degrees, heading, error, heading_diff))
# check if the heading isn't so far from the desired orientation
# if it needs correction, then rotate the robot
if error >= acceptable_error_percent and abs(
how_much_to_rotate) >= accepted_minimum_by_drivers:
gopigo3_robot.turn_degrees(how_much_to_rotate, blocking=True)
# command for making the robot move of stop
if move is False:
gopigo3_robot.stop()
else:
gopigo3_robot.forward()
# we are reading and displaying the distance here so it only reads when you are moving.
#If space bar is hit and the robot is no longer moving, it will not read distance
dstnc = my_distance_sensor.read_mm()
print("Distance Sensor Reading: {} mm ".format(dstnc))
sleep(0.001)
# if the synchronization wasn't broken
# then stop the motors in case they were running
if not simultaneous_launcher.broken:
gopigo3_robot.stop()
#
# This code is an example for controlling the GoPiGo3 motors. This uses
# the EasyGoPiGo3 library. You can find more information on the library
# here: http://gopigo3.readthedocs.io/en/latest/api-basic.html#easygopigo3
#
# Results: The GoPiGo3 will move forward for 2 seconds, and then
# backward for 2 second.
# import the time library for the sleep function
import time
# import the GoPiGo3 drivers
import easygopigo3 as easy
# Create an instance of the GoPiGo3 class.
# GPG will be the GoPiGo3 object.
gpg = easy.EasyGoPiGo3()
print("Move the motors forward freely for 1 second.")
gpg.forward()
time.sleep(1)
print("Stop the motors for 1 second.")
gpg.stop()
time.sleep(1)
print("Drive the motors 50 cm and then stop.")
gpg.drive_cm(50, True)
print("Wait 1 second.")
time.sleep(1)
import time
import random
import easygopigo3 as easy
box_count = 0
gpg = easy.EasyGoPiGo3() #instatiating EasyGoPiGo3 object
dist_sensor = gpg.init_distance_sensor(
) #instance of the Distance Sensor class
servo = gpg.init_servo() #instance of the Servo class
BOX_DIST = 30 #max distance of box detection in centimeters
forward_dist = 70 #distance car moves before stopping to scan for boxes
#put car in the default state
servo.reset_servo()
time.sleep(1)
gpg.stop()
while True:
#scan area for boxes
#point servo right and collect distance data
servo.rotate_servo(0)
right_obj_dist = dist_sensor.read()
time.sleep(1)
#point servo left and collect distance data
servo.rotate_servo(180)
left_obj_dist = dist_sensor.read()
time.sleep(1)
