def __init__(self, state):
self.motorControl = motorControl(state)
self.state = state
self.cd = captorDistance()
#self.all_or_nothin_follow()
#self.up_to_point_follow()
self.two_points_follow()
def __init__(self, mir=mirConnection()):
# mission_id for methods GoToA, GoToStart, and GoToSafe for mir
self.GoToA = "cfbc2ff2-0363-11eb-99a6-000129922c9e"
self.GoToStart = "94601816-0363-11eb-99a6-000129922c9e"
self.GoToSafe = "130b5cac-17dc-11eb-89d2-000129922c9e"
self.mir = mir
self.motor = motorControl()
self.previous = None
self.hasDocked = False
self.end = False
self.command = ''
self.hasDocked = False
self.safetyE = threading.Event()
self.safetyE.set()
fn = "selectedMoves_20190813-200118.pkl"
trajData = pickle.load(open(fn, "rb"))
position, orientation = trajData[-1]
# Starting at 1ft by 1 ft
oneFt = retrieval.convertIn2Meters(12)
startingPos = position
startingOrient = orientation #Deg
#Drop Off Zone Center at 2ft * 10ft
#dropOffPos = (1*oneFt, 3*oneFt)
dropOffPos = (1*oneFt,10*oneFt)
#Initialize Components
myGrip = grip.gripper()
myMotor= motors.motorControl(initPosOrient
= (startingPos[0], startingPos[1], startingOrient))
mySodar = sodar.sodar()
maskBoundsRGB = {}
maskBoundsRGB_orig = {}
# Red Block HSV Mask
minH = 0#0
minS = 142#158
minV = 71#92
maxH = 4#180
maxS = 255#216
maxV = 255#155
minHSV = (minH, minS, minV)
maxHSV = (maxH, maxS, maxV)
from motorControl import motorControl
# Keyboard
import tty
import sys
import termios
orig_settings = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin)
key = 0
oldKey = 0
speed = 100
m = motorControl()
m.begin()
print('Press ESC to exit')
while key != chr(27): #ESC
key = sys.stdin.read(1)[0]
if key != oldKey:
# Stop
if key == 's':
m.setSpeed(0, 0)
m.setSpeed(1, 0)
m.setSpeed(2, 0)
m.setSpeed(3, 0)
# Move forward
to_target = target - frame_center
to_target = [int(x) for x in to_target]
print("[{}, {}]".format(to_target[0], to_target[1]))
return to_target
if __name__ == '__main__':
with PiCamera() as camera:
camera.resolution = (_IMAGE_WIDTH, _IMAGE_HEIGHT)
camera.framerate = _FRAMERATE
rawCapture = PiRGBArray(camera, size=camera.resolution)
time.sleep(0.1) # let camera warm up
with motorControl([13, 19], [20, 16]) as motCon:
# capture_continuous returns infinite iterator of images
# use_video_port for rapid capture
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
image = frame.array
contours = getContours(image)
if len(contours) > 0:
maxc = max(contours,
key=cv2.contourArea) # get biggest contour
rect = cv2.minAreaRect(maxc)
rectw, recth = rect[1]
# check size of contour
if rectw < _LENGTH_THRESH and recth < _LENGTH_THRESH:
#-------------------
#----BUZZLED7------
#-------------------
# Obtain Time
now = datetime.datetime.now()
hour_str = now.strftime("%H")
minute_str = now.strftime("%M")
print "Hour: " + hour_str + " Minute: " + minute_str
# Messages on LED
msg = hour_str[0] + hour_str[1] + minute_str[0] + minute_str[1]
display = led7.led7()
display.clean()
display.showValue(msg)
#display.showDebug (msg)
#-------------------
#-----BUZZBOT-------
#-------------------
bot = motorControl.motorControl()
bot.stopBot()
# Move forward and backwards, then stop
bot.moveByStep(-2, 2)
print("Move by step")
# Move continuously
#bot.moveForward(True)
#bot.moveBackwards(True)
bot.stopBot()
def __init__(self, Ti, state):
self.Ti = Ti
self.mc = motorControl(state)
self.state = state
self.leading(40)
break
print("Block Color is: ", colorUsed)
print("Center on Block: Success:" , success)
return success, COI, colorUsed
if __name__ == "__main__":
sys.path.insert(0, '/home/pi/enpm809T/gripper_toolbox/')
sys.path.insert(0, '/home/pi/enpm809T/motorControl_toolbox/')
sys.path.insert(0, '/home/pi/enpm809T/sodar_toolbox/')
import gripper as grip
import motorControl as motors
import sodarMeasure as sodar
myGrip = grip.gripper()
myMotor= motors.motorControl()
mySodar = sodar.sodar()
maskBoundsRGB = {}
# Red Block HSV Mask
minH = 0#0
minS = 70#158
minV = 50#92
maxH = 10#180
maxS = 255#216
maxV = 255#155
minHSV = (minH, minS, minV)
maxHSV = (maxH, maxS, maxV)
maskBoundsRGB['r'] = (minHSV, maxHSV)
import motorControl
myMotor = motorControl.motorControl()
"""
for i in range(0,4):
myMotor.forward(.45, 35)
myMotor.pivotLeftAng(90, 75)
"""
"""
myMotor.pivotLeftAng(90, 75)
myMotor.reverse(.3, 35)
myMotor.pivotRightAng(225, 75)
myMotor.forward(.3, 35)
"""
def convertIn2Meters(distIn):
return 0.0254 * distIn
myMotor.forward(convertIn2Meters(8), 35)
myMotor.pivotRightAng(40, 75)
myMotor.reverse(convertIn2Meters(4), 35)
myMotor.pivotLeftAng(90, 75)
myMotor.forward(convertIn2Meters(4), 35)
#-------------------
#----BUZZLED7------
#-------------------
# Obtain Time
now = datetime.datetime.now()
hour_str = now.strftime("%H")
minute_str = now.strftime("%M")
print "Hour: " + hour_str + " Minute: " + minute_str
# Messages on LED
msg = hour_str[0] + hour_str[1] + minute_str[0] + minute_str[1]
display = led7.led7()
display.clean()
display.showValue (msg)
#display.showDebug (msg)
#-------------------
#-----BUZZBOT-------
#-------------------
bot = motorControl.motorControl()
bot.stopBot()
# Move forward and backwards, then stop
bot.moveByStep(-2,2)
print ("Move by step")
# Move continuously
#bot.moveForward(True)
#bot.moveBackwards(True)
bot.stopBot()