def DEBUGwithWait():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
#print ('Program stop for DEBUG\nSeconds: ', seconds() - startTime)
ao()
msleep(5000)
exit(0)
def drive_timed(lmotor, rmotor, time):
print("driving timed")
_clear_ticks()
end = seconds() + time
if lmotor == 0 or rmotor == 0:
print("please use pivot instead!")
elif abs(rmotor) <= abs(lmotor):
mod = rmotor / (lmotor * 1.0)
newLeftSpeed = lmotor
newRightSpeed = int(mod * lmotor)
elif abs(lmotor) < abs(rmotor):
mod = (lmotor * 1.0) / rmotor
newLeftSpeed = int(mod * rmotor)
newRightSpeed = rmotor
while seconds() <= end:
if int(_right_ticks() / mod) == int(_left_ticks() / mod):
_drive(newLeftSpeed, newRightSpeed)
if int(_right_ticks() / mod) > int(_left_ticks() / mod):
_drive(newLeftSpeed, int(newRightSpeed / 1.3))
if int(_left_ticks() / mod) > int(_right_ticks() / mod):
_drive(int(newLeftSpeed / 1.3), newRightSpeed)
freeze_motors()
print(get_motor_position_counter(RMOTOR))
ao()
def DEBUG_WITH_WAIT():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
ao()
msleep(5000)
display('Program stop for DEBUG\nSeconds: {}'.format(seconds() -
c.startTime))
exit(0)
def DEBUG():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
ao()
display('Program stop for DEBUG\nSeconds: {}'.format(seconds() -
c.startTime))
display("NOTE: {}\t{}".format(seconds(), c.startTime))
exit(0)
def move_to_med(): # move to medical center
print "Moving towards the %s medical center" % (
("close", "far")[c.burning_center])
d.degreeTurn(50, ((-80, -70)[c.last_direction],
(-95, -85)[c.last_direction])[c.burning_center])
d.skipLine(50, c.largeTopHat.port(), c.LARGE_TOPHAT_LINE,
(1, 3)[c.burning_center])
print "skipped the line!"
d.forward(50, 1000)
print "moving forward!"
w.ao()
w.msleep(100)
d.backward(50, 2000)
print "moving backward!"
def ao(self, log=True):
if(log and self._logging):
self.printf("ALL OFF")
self.stop(False)
wallaby.ao()
def shutdown():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
ao()
display('Program stopped\nSeconds: {}'.format(seconds() - c.startTime))
exit(0)
def PROGRAMMER_ERROR(msg) :
ao()
print msg
exit()
def stop():
ao()
def crossBlackFront():
while not onBlackFront(): # wait for black
pass
while onBlackFront(): # wait for white
pass
ao()
def DEBUG():
ao()
print 'Program stop for DEBUG\nSeconds: ', seconds() - c.startTime
exit(0)
def DEBUGwithWait():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
ao()
print 'Program stop for DEBUG\nSeconds: ', seconds() - c.startTime
msleep(5000)
def crossBlackBack():
while not onBlackBack(): # wait for black
pass
while onBlackBack(): # wait for white
pass
ao()
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()
def DEBUG():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
print('Program stop for DEBUG\nSeconds: ', seconds() - c.startTime)
ao()
exit(0)
def _stop(): # Turns off all the motors.
ao()
def PROGRAMMER_ERROR(msg, value) :
ao()
print msg, value
exit()
