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()