def deliverBin():
print("deliverBin")
drive(-100, 0)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(100, 100, 500)
def grabBin():
print("grabBin")
moveOutrigger(c.outriggerBin, 20)
driveTimed(0, -100, 500)
drive(-95, -100)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(-50, -50, 150)
binGrabUp()
def turnToSouth():
print("turnToSouth")
driveTimed(90, 100, 1000)
deliverPoms()
drive(100, 0)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(100, 0, 500)
moveOutrigger(c.outriggerOut, 100)
def arc_right(dist, power=100):
"""
Sweeping turn to the right
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist:
drive.move(power, power - 20)
drive.stop()
def flagGrab():
drive.moveBackward()
time.sleep(1)
drive.stop()
drive.turnLeft90()
drive.turnLeft90()
release()
drive.moveBackward()
time.sleep(1)
drive.stop()
grab()
def goToTaterBin():
print("goToTaterBin")
moveArm(c.armMid, 10)
drive(95, 100)
moveOutrigger(c.outriggerApproach, 20)
while not onBlack(c.STARBOARD_TOPHAT):
pass
drive(25, 30)
while not atArmLength():
pass
stop()
def move(self):
if (self.direction == 'backwards'):
if (self.requested_speed > 0):
drive.reverse(self.requested_speed)
else:
drive.stop()
else:
if (self.requested_speed > 0):
drive.forward(self.requested_speed)
else:
drive.stop()
def grabCube():
print("grabCube")
moveArm(c.armUp, 10)
drive(0, -100)
crossBlack(c.LINE_FOLLOWER)
moveClaw(c.clawOpen, 15)
msleep(500)
stop()
moveArm(c.armFront, 15)
driveTimed(100, 100, 1100)
binGrabUp()
moveClaw(c.clawClose, 10)
def scoreCube():
print "scoreCube"
drive(50, 50)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
msleep(500)
driveTimed(45, 50, 700)
moveClaw(c.clawOpen, 25)
msleep(300)
moveArm(c.armUp, 10)
driveTimed(100, 100, 1100)
def grabMiddlePile():
print("grabMiddlePile")
moveClaw(c.clawOpen, 25)
moveArm(c.armFront, 25)
drive(100, 100)
msleep(300)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 3)
moveClaw(c.clawMid, 10)
drive(60, 60)
moveClaw(c.clawClose, 5)
moveArm(c.armMid, 25)
stop()
deliverPoms()
def align_encoder():
"""
Ensure both wheels are aligned on encoders
"""
leftWheel.reset()
while leftWheel.count < 1:
drive.move(40, 0)
drive.stop()
rightWheel.reset()
while rightWheel.count < 1:
drive.move(0, 40)
drive.stop()
def grabSouthPile():
print ("grabSouthPile")
moveClaw(c.clawOpen, 10)
moveArm(c.armFront, 15)
moveArm(c.armShovel, 10)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 5)
drive(50, 50) #now same on both
moveArm(c.armFront, 50)
moveClaw(c.clawClose, 5)
msleep(500)
stop()
moveArm(c.armMid, 15)
deliverPoms()
moveOutrigger(c.outriggerFindLine, 25)
def main():
while True:
try:
uValueC = BP.get_sensor(BP.PORT_2)
except brickpi3.SensorError as error:
print(error)
if(uValueC == 0):
drive.stop()
else:
if(uValueC <= 5):
drive.turnLeft90()
checkObject()
else:
drive.moveForward()
time.sleep(0.02)
def grabNorthPile():
print("grabNorthPile")
drive(93, 100)#90
moveClaw(c.clawMid, 10)
msleep(1000)
drive(54, 50)#45,50
moveClaw(c.clawClose, 4)
msleep(500)
stop()
#moveArm(c.armMid, 15)
#msleep(500)
moveClaw(c.clawOpen, 10)
msleep(500)
moveClaw(c.clawClose, 10)
'''driveTimed(-50, -50, 1000)
def avoidance():
uValueC = 70
try:
uValueC = BP.get_sensor(BP.PORT_2)
except brickpi3.SensorError as error:
print(error)
if (uValueC == 0):
drive.stop()
return False
else:
if (uValueC <= 5):
drive.turnLeft90()
return True
else:
drive.moveForward()
return False
def goToNorthernPile():
print("goToNorthernPile")
moveClaw(c.clawOpen, 30)
moveArm(c.armMid, 20)
drive(100, 90)
while not onBlack(c.STARBOARD_TOPHAT):
pass
driveTimed(100, 100, 150)
moveOutrigger(c.outriggerTurn, 20)
drive(100, -20) #was drive(100, -20)
while not onBlack(c.STARBOARD_TOPHAT):
pass
moveArm(c.armFront, 10)
if c.isClone:
driveTimed(0, 100, 200) #300
stop()
driveTimed(0, 100, 300)
def reverse_dist(dist, power):
"""
Travel a given distance in reverse
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist or rightWheel.count < dist:
log("leftRev={0} vs rightRev={1}".format(leftWheel.count, rightWheel.count))
if leftWheel.count < rightWheel.count:
log("Teak right")
drive.move(-power, 0)
elif leftWheel.count > rightWheel.count:
log("Teak left")
drive.move(0, -power)
else:
log("No tweaks")
drive.move(-power, -power)
drive.stop()
def forward_dist(dist, power=100):
"""
Travel forwards a given distance
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist or rightWheel.count < dist:
log("leftFwd={0} vs rightFwd={1}".format(leftWheel.count, rightWheel.count))
if leftWheel.count < rightWheel.count:
log("Teak right")
drive.move(power, 0)
elif leftWheel.count > rightWheel.count:
log("Teak left")
drive.move(0, power)
else:
log("No tweaks")
drive.move(power, power)
drive.stop()
log("Distance reached so stopping")
def goToCenterAgain():
print("goCenterAgain")
driveTimed(95, 100, 3000)
DEBUG()
driveTimed(100, 60, 3000)
drive(100, 100)
while not onBlack(c.LINE_FOLLOWER):
pass
drive(100, 0)
while onBlack(c.LINE_FOLLOWER):
pass
stop()
timedLineFollowRight(c.LINE_FOLLOWER, 2)
drive(100, 100)
moveClaw(c.clawOpen, 25)
lineFollowUntilEndRight(c.LINE_FOLLOWER)
driveTimed(100, 0, 150)
msleep(400)
driveTimed(100, 100, 1500)
def avoidance(self):
uValue = 70
uValue = self.getUltrasonic()
if (uValue == 0):
drive.stop()
else:
if (uValue <= 3):
self.direction = random.randint(0, 1)
if (self.direction == 0):
drive.turnLeft45()
drive.pivotTurn45(30, 10)
self.h.turnRight()
time.sleep(1)
else:
drive.turnRight45()
drive.pivotTurn45(10, 30)
self.h.turnLeft()
time.sleep(1)
self.switcher = 1
else:
drive.moveForward()
def returnToBase():
print ("returntobase")
moveArm(c.armBlockBack, 10)
driveTimed(-100, 0, 1000)
drive(-100, -87) #-85
while not onBlack(c.STARBOARD_TOPHAT):
pass
driveTimed(-100, 0, 300)
timedLineFollowBack(c.STARBOARD_TOPHAT, 3)#2
driveTimed(-80, -100, 1000)
drive(-90, -100)
msleep(3500);
moveOutrigger(c.outriggerBaseReturn, 20)
msleep(1000)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
msleep(10)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
msleep(10)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
stop()
def linefollowing():
BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED)
sensor = 0
elapsedTime = 0
maxTime = 17
startTime = time.time()
while (sensor < 90):
try:
sensor = BP.get_sensor(BP.PORT_3)
# print(sensor)
except brickpi3.SensorError as error:
print(error)
elapsedTime = round(time.time() - startTime, 1)
if (elapsedTime <= maxTime):
turning(sensor)
else:
drive.revPivotTurn45(-30, -15)
drive.moveFast()
time.sleep(0.1)
drive.stop()
break
time.sleep(0.02)
def goToCenter():
print("goToCenter")
if c.isPrime:
driveTimed(95, 100, 4000)
driveTimed(100, 60, 3000) #3000
else:
driveTimed(90, 100, 4000)
driveTimed(100, 60, 3000)
drive(100, 100)
while not onBlack(c.LINE_FOLLOWER):
pass
drive(100, 0)
while onBlack(c.LINE_FOLLOWER):
pass
stop()
timedLineFollowRight(c.LINE_FOLLOWER, 1)
drive(100, 100)
moveClaw(c.clawOpen, 25)
lineFollowUntilEndRight(c.LINE_FOLLOWER)
driveTimed(90, 50, 200)
moveArm(c.armShovel, 15)
msleep(400)
driveTimed(100, 100, 1500)
def turn_to_cubes():
if not w.camera_open():
return
servo_centered = False
c.camera_servo.enable()
while not servo_centered: # first center the
w.camera_update()
objects = w.get_object_count(c.YELLOW)
if objects == 0:
print "no objects!"
c.camera_servo.setPosition(2047)
continue
best = x.getGreatest(c.YELLOW)
if w.get_object_confidence(c.YELLOW, best) < 0.5:
continue
print "Cube:",
print w.get_object_center_x(c.YELLOW, best),
print w.get_object_confidence(c.YELLOW, best)
x.centerX_servo(c.YELLOW, best, 10, 50) # first find it with our servo
if (w.get_camera_width() / 2) - 25 < w.get_object_center_x(
c.YELLOW, best) < (w.get_camera_width() / 2) + 10:
servo_centered = True
break
servo_pos = c.camera_servo.position()
print servo_pos
print c.SERVO_TICK2DEG(servo_pos)
print int(c.SERVO_TICK2DEG(servo_pos))
print int((c.SERVO_TICK2DEG(servo_pos)) - 90)
d.forward(50, 2100)
d.stop()
w.msleep(250)
d.degreeTurn(50, int(c.SERVO_TICK2DEG(servo_pos) - 75))
c.camera_servo.setPosition(900) # reset camera to default position
u.moveDegree(c.spinner.port(), 50, 90) # set our sweeper to default pos
c.distance_traveled = 0 # clear distance
w.camera_close()
def right90():
"""
Turn robot right 90 degrees by running left wheel
"""
dist = 21.0
drive.stop()
rightWheel.reset()
leftWheel.reset()
move_state = True
while move_state:
log("target= {0} v actual= {1}".format(dist, leftWheel.dist))
if dist < leftWheel.dist:
drive.stop()
move_state = False
else:
drive.move(40, 0)
drive.stop()
def left90():
"""
Turn the robot 90 degrees left by running the right hand motor
"""
dist = 19.0
drive.stop()
rightWheel.reset()
leftWheel.reset()
move_state = True
while move_state:
log("target= {0} v actual= {1}".format(dist, rightWheel.dist))
if dist < rightWheel.dist:
drive.stop()
move_state = False
else:
drive.move(0, 40)
drive.stop()
# Put encoder to known position
align_encoder()
# drive from A to B
drive.SetSpeed(speed, speed);
print(encoder.get())
drive.sleep(1)
# turn towards C
drive.SetSpeed(-speed,speed);
drive.sleep(0.7)
#drive from B to C
drive.SetSpeed(speed, speed);
drive.sleep(1.5)
# turn towards D
drive.SetSpeed(speed,-speed);
drive.sleep(0.7)
# drive from C to D
drive.SetSpeed(speed, speed);
drive.sleep(1)
# stop
drive.stop();
while 1:
sleep(10)
drive.gpioCleanUP()
def destroy(): # Shutdown GPIO and Cleanup modules
print('\n... Shutting Down...\n')
drive.stop() # Make sure Bot is not moving when program exits
drive.cleanup() # Shutdown all motor control
GPIO.cleanup()
def avoidance(self):
uValue = 70
uValue = self.getUltrasonic()
print("Avoidance:" + str(uValue))
if (uValue <= 10):
self.closeToObject = True
if (uValue == 0):
drive.stop()
else:
if (self.closeToObject == True):
if (self.ScanValues[2] == 0):
self.h.returnCentre()
drive.stop()
time.sleep(1)
self.ScanValues[2] = self.getUltrasonic()
print("Centre Scan Value:" + str(uValue))
if (self.i <= 9 and self.ScanValues[1] == 0):
if (self.i == 0):
scanV = self.h.scan(0)
else:
scanV = 3
if (scanV == 2):
self.leftScanArray.append(self.getUltrasonic())
elif (scanV == 3):
if (self.ScanValues[0] == 0):
self.ScanValues[0] = self.getLowest(
self.leftScanArray)
print("Left Scan Value:" + str(self.ScanValues[0]))
self.rightScanArray.insert(self.i,
self.getUltrasonic())
self.i += 1
print("i = " + str(self.i))
# print("Right Scan Value:" + str(self.getUltrasonic()))
elif (self.ScanValues[1] == 0):
self.ScanValues[1] = self.getLowest(self.rightScanArray)
print("Right Scan Value:" + str(self.ScanValues[1]))
self.i = 0
elif (self.positionSet == False):
if (self.ScanValues[1] > 60):
self.h.turnLeft(0.7)
self.direction = 1
self.headAngle = 1
elif (self.ScanValues[0] > 60):
self.h.turnRight(0.7)
self.direction = 0
self.headAngle = 2
else:
self.h.returnCentre()
self.direction = random.randint(0, 1)
self.headAngle = 0
self.positionSet = True
else:
if (uValue < 5):
if (self.headAngle == 0):
if (self.direction == 0):
drive.turnLeft90()
self.h.turnRight(1)
else:
drive.turnRight90()
self.h.turnLeft(1)
elif (self.headAngle == 1):
drive.turnRight45()
self.h.returnCentre()
self.h.turnLeft(1)
else:
drive.turnLeft45()
self.h.returnCentre()
self.h.turnRight(1)
self.switcher = 1
else:
drive.moveForward()
else:
drive.moveForward()
self.h.scan(1)
def stop(self):
drive.stop()
self.manager.lbl_status.text = "Engine stopped"
def leftTurn(time=5):
drive.drive("L", 50, 2, 0)
drive.drive("R", 50, 2, 1)
time.sleep(time)
drive.stop()
decision, pixel_width = tracking.steering_decision(frame, LOWER, UPPER,
STEERING_TOLERANCE,
HEIGHT, WIDTH)
if pixel_width != 0:
distance_prediction = tracking.distance(pixel_width, REAL_WIDTH,
FOCAL_LENGTH)
else:
distance_prediction = "No object"
print(decision, distance_prediction)
# Actions, based on previous decision
if decision == "stay":
drive.stop()
elif decision == "left":
drive.turn_left(0.01)
elif decision == "right":
drive.turn_right(0.01)
else:
drive.stop()
if distance_prediction != "No object":
if abs(distance_prediction -
DESIRED_DISTANCE) > DESIRED_DISTANCE_TOLERANCE:
if distance_prediction - DESIRED_DISTANCE < 0:
drive.backward(0.015)
else:
drive.forward(0.015)
else:
def move_to_cubes():
c.camera_servo.setPosition(900)
d.forward(50, 500)
w.msleep(250)
d.degreeTurn(50, 70)
u.moveDegree(c.spinner.port(), 50, 90)
c.distance_traveled = 0 # reset distance
if not w.camera_open():
return
at_cubes = False
ol = -1
while not at_cubes:
w.camera_update()
objects = w.get_object_count(c.YELLOW)
if objects == 0:
print "no objects!"
c.can_see = False
if ol == 2:
if c.last_direction == 0:
if c.last_seen_x < w.get_camera_width() + 5:
d.spinLeft(10, 200)
d.forward(10, 200)
else:
if c.last_seen_x < w.get_camera_width() - 5:
d.spinRight(10, 200)
d.forward(10, 200)
w.ao()
at_cubes = True
print "No objects, but passed line!"
break
continue
c.can_see = True
best = x.getGreatest(c.YELLOW)
c.last_seen_x = w.get_object_center_x(c.YELLOW, best)
x.centerX(c.YELLOW, best)
print ol,
if u.isOnLine(c.largeTopHat.port(), c.LARGE_TOPHAT_LINE) and ol == -1:
ol += 1
print "Hitting first line"
elif not u.isOnLine(c.largeTopHat.port(),
c.LARGE_TOPHAT_LINE) and ol == 0:
ol += 1
print "Passed First line"
elif u.isOnLine(c.largeTopHat.port(), c.LARGE_TOPHAT_LINE) and ol == 1:
d.stop()
c.collection_arm.setPosition(1600)
w.msleep(500)
c.collection_arm.setPosition(270)
w.msleep(500)
ol += 1
print "Hitting second line"
elif not u.isOnLine(c.largeTopHat.port(),
c.LARGE_TOPHAT_LINE) and ol == 2:
d.stop()
at_cubes = True
if c.last_direction == 0:
if c.last_seen_x < w.get_camera_width() + 10:
d.spinLeft(10, 200)
d.forward(10, 200)
else:
if c.last_seen_x < w.get_camera_width() - 10:
d.spinRight(10, 200)
d.forward(10, 200)
print "Passed second line"
break
print "Cube:",
print w.get_object_center_x(c.YELLOW, best),
print w.get_object_confidence(c.YELLOW, best)
w.camera_close()
#--- stockpile
["stack", lambda: stackFile()],
["dump feeder material", lambda: dump_feeder.dump1()],
["flush feeder material", lambda: dump_feeder.dump2()],
["reclaim", lambda: reclaimer.reclaim("../data/" + lastStockpileName)],
#--- feeder
["control feeder", "./shakekey.sh"],
["stop feeder", lambda: shaker.stop()],
#--- drive
["flush stockpile", lambda: drive.flush()],
["drive to start pos", lambda: drive.moveToStockpile(0)],
["drive to end pos", lambda: drive.moveToStockpile(1)],
["drive to dump pos", lambda: drive.moveToMM(-400)],
["drive left", lambda: drive.run(-1000)],
["drive right", lambda: drive.run(1000)],
["drive stop", lambda: drive.stop()],
#--- other
["colorcount", lambda: os.system("colorcount")],
["alarm", "./alarm.sh"],
["poweroff", lambda: os.system("poweroff")],
["reboot", lambda: os.system("reboot")],
["close", lambda: stop()]
]
def stop():
global run
run = False
return 0
def stackFile():
global lastStockpileName
def on_press(key):
#frame = vs.read()
frame = 'Evangelion'
if key == Key.esc:
drive.stop()
cv2.destroyAllWindows()
#vs.stop()
return False
if key == Key.up:
drive.move(Forward)
print('foward')
printtofile(frame, 'forward')
if key == Key.down:
drive.move(Reverse)
print('reverse')
printtofile(frame, 'reverse')
if key == Key.left:
drive.move(Left)
print('left')
printtofile(frame, 'left')
if key == Key.right:
drive.move(Right)
print('right')
printtofile(frame, 'right')
if key == Key.page_up:
drive.move(F_Right)
print('f_right')
printtofile(frame, 'f_right')
if key == Key.home:
drive.move(F_Left)
print('f_left')
printtofile(frame, 'f_left')
if key == Key.page_down:
drive.move(R_Right)
print('r_right')
printtofile(frame, 'r_right')
if key == Key.end:
drive.move(R_Left)
print('r_left')
printtofile(frame, 'r_left')
if key == Key.f1:
drive.move(Spin_F_Left)
print('spot_f_left')
printtofile(frame, 'spin_f_left')
if key == Key.f2:
drive.move(Spin_F_Right)
print('spin_f_right')
printtofile(frame, 'spin_f_left')
if key == Key.f3:
drive.move(Spin_R_Left)
print('spin_r_left')
printtofile(frame, 'spin_r_left')
if key == Key.f4:
drive.move(Spin_R_Right)
print('spin_r_right')
printtofile(frame, 'spin_r_right')
if key == Key.f5:
drive.move(Spot_F_Right)
print('spot_f_right')
printtofile(frame, 'r_left')
if key == Key.f6:
drive.move(Spot_F_Left)
print('spot_f_left')
printtofile(frame, 'spot_f_left')
if key == Key.f7:
drive.move(Spot_R_Right)
print('spot_r_right')
printtofile(frame, 'spo_r_left')
if key == Key.f8:
drive.move(Spot_R_Left)
print('spot_r_left')
printtofile(frame, 'r_left')
#if key == Key.enter:
# drive.stop()
# print('stop')
# printtofile(frame,'stop')
if key == Key.delete:
drive.stop()
drive.cleanup()
print('STOP AND CLEANUP')
