def charge_and_swerve_F_minus(R): center_marker = 32 if R.zone == 0: # Zone A close_marker = 33 far_marker = 34 else: # Zone B close_marker = 34 far_marker = 33 move.forward_dist(R, 0.9) move.slowly_stop_all_motors(R) move.scoop_left(R, 130) can_see_marker = False time.sleep(3) markers = R.see() print "I see", len(markers) for marker in markers: print marker.info.code, marker.dist if marker.info.code == far_marker: # The marker in the opposite zone # move towards marker move.forward(R, marker.dist) can_see_marker = True if not can_see_marker: print("Cannot see")
def square(): #18.6046511628 current = (0, 0, 0) for i in range(0, 4): for j in range(4): forward(10) previous_pos = current particles = generate_particles_from_movement(current, 10) avgX = sum([x for (x, y, theta) in particles]) / 100 avgY = sum([y for (x, y, theta) in particles]) / 100 avgTheta = sum([theta for (x, y, theta) in particles]) / 100 current = (avgX, avgY, avgTheta) line = (previous_pos[0], previous_pos[1], avgX, avgY) #plot the points print("drawLine:" + str(line)) print("drawParticles:" + str(particles)) print("###########################WENT FORWARD#################") time.sleep(0.2) left(90) print("##############TURNED LEFT######################") particles = generate_particles_from_turn(current, 90) #plot the new points print("drawParticles:" + str(particles)) time.sleep(0.1)
def directions(instructions): if instructions[0] == "Forward": move.forward() elif instructions[0] == "Left": move.left() elif instructions[0] == "Right": move.right() elif instructions[0] == "Standing": move.stand() elif instructions[0] == "Auto": move.autonom()
def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal): T = 15000.0 # max simulation goal_dis = 0.2 stop_speed = 0.1 state = modelito.State(x=0.15, y=0.15, yaw=6, v=0.0) time = 0.0 x = [state.x] y = [state.y] yaw = [state.yaw] v = [state.v] t = [0.0] target_ind = calc_target_index(state, cx, cy) while T >= time: di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) ai = PIDControl(speed_profile[target_ind], state.v) state = modelito.update(state, ai, di) if abs(state.v) <= stop_speed: target_ind += 1 time = time + modelito.dt dx = state.x - goal[0] dy = state.y - goal[1] if math.sqrt(dx**2 + dy**2) <= goal_dis: print("Logrado") break x.append(state.x) y.append(state.y) yaw.append(state.yaw) v.append(state.v) move.forward(100 * abs(state.v), 100 * abs(state.v * np.cos(state.yaw))) t.append(time) return t, x, y, yaw, v
def navigateToWaypoint(X, Y, current): #X is desired X,Y is desired Y #assuming we have access to our x,y,theta values (position and direction of robot) #take dY = Y-y;dX = X-x #we need to turn (phi - theta) degrees with phi = atan2(dY,dX). #then move forward a distance of sqrt(pow(dY,2)+pow(dX,2)) dY = Y - current[1] dX = X - current[0] print(dY) print(dX) phi = math.atan2(dY, dX) dist = math.sqrt(math.pow(dY, 2) + math.pow(dX, 2)) if dX > 0: angle = phi - current[2] #align with point if dX +ve else: angle = phi - (current[2] - math.pi) #offset by pi if dX -ve print(angle) print(dist) right(angle) forward(dist) #idk if this is how it works in python current[0] = current[0] + dX current[1] = current[1] + dY current[3] = current[3] + phi
interface = brickpi.Interface() interface.initialize() speed = -6.0 motors = [0,3] interface.motorEnable(motors[0]) interface.motorEnable(motors[1]) motorParams = interface.MotorAngleControllerParameters() motorParams.maxRotationAcceleration = 6.0 motorParams.maxRotationSpeed = 12.0 motorParams.feedForwardGain = 255/20.0 motorParams.minPWM = 18.0 motorParams.pidParameters.minOutput = -255 motorParams.pidParameters.maxOutput = 255 motorParams.pidParameters.k_p = 540 motorParams.pidParameters.k_i = 2000 motorParams.pidParameters.K_d = 34 interface.setMotorAngleControllerParameters(motors[0], motorParams) interface.setMotorAngleControllerParameters(motors[1], motorParams) forward(30) interface.setMotorRotationSpeedReferences(motors,[speed,speed]) while not interface.motorAngleReferencesReached(motors) : time.sleep(0.1)
def move_asynch(chosen_path, state): global DUMPED, NEXT_NODE instruction = None # Calling stop on this thread throws ThreadKiller at the current point of # execution, wrapping all of the logic in a try catch allows for cleanup and # prevents the stack trace. try: while True: # Get the next instruction instruction = chosen_path.pop(0) success = True # Dispatch to the relavent functions depending on the command if isinstance(instruction, Move): #print("moving") success = forward(instruction.dist, tolerance = instruction.tolerance) elif isinstance(instruction, Dump): # Communication between the two bricks was turned off during # profiling if not PROFILING: # Instruct the second brick to dump CLIENT.publish("dump", json.dumps(instruction.slots)) # Spin until it replies while True: with dumped_lock: if DUMPED: DUMPED = False break elif isinstance(instruction, Rotate): #print("rotating") # The route planner always generates instructions telling the # robot to turn right, this adapts to turn left when that is # more efficient if instruction.angle <= 180: direction = Directions.ROT_RIGHT angle = instruction.angle else: direction = Directions.ROT_LEFT angle = instruction.angle - 180 success = rotate(angle, tolerance = instruction.tolerance, direction = direction) elif isinstance(instruction, ToDesk): #print("approaching desk") angle = instruction.angle if instruction.is_left: direction = Directions.ROT_LEFT else: direction = Directions.ROT_RIGHT # This function unconditionally turns 90 degrees and so can't # fail approach(angle=angle, direction=direction) elif isinstance(instruction, FromDesk): #print("leaving desk") angle = instruction.angle if instruction.is_left: direction = Directions.ROT_LEFT else: direction = Directions.ROT_RIGHT # On the way back however it searches for a line so can success = approach(angle=angle, tolerance=instruction.tolerance, direction=direction, reverse=True) elif isinstance(instruction, Report): #print("reporting") CLIENT.publish("location_info", payload=instruction.where) # If an instruction failed panic if not success: #print("panicking") STATE_QUEUE.put(T_PANICKING) break # If we ran out of instructions without panicking make a transition # depending on what state we are currently in if len(chosen_path) == 0: if state == State.DELIVERING: #print("Returning") STATE_QUEUE.put(T_RETURNING) #print(STATE_QUEUE) break elif state == State.RETURNING: #print("Loading") STATE_QUEUE.put(T_LOADING) break # Last reported location for return with next_node_lock: if isinstance(instruction, Report): NEXT_NODE = instruction.where #print(NEXT_NODE) # TODO right now the code spins here forever after executing the movement # commands - does not need to while True: pass except ThreadKiller as e: # There is a certain amount of unreliability in the method used to kill # threads (See thread_decorator.py) so the code keeps sending exceptions # until the thread dies or calls acknowledge. After a call to # acklowledge the thread is allowed as much time as it needs to cleanup. acknowledge(e) stop_motors() final = [] # Resolve the various instructions where we could have been interupted if isinstance(instruction, Move): # Inside move figure out how far we still need to go and store that # for next time. (If it is too small search for the junction # anywhere between 0 and 20) dist = instruction.dist - get_odometry() if dist <= 20: final = [Move(20, 100)] else: final = [Move(dist, 50)] # If there is a rotate next also take that if chosen_path and isinstance(chosen_path[0], Rotate): final.append(chosen_path.pop(0)) elif isinstance(instruction, Dump): # If it's a dump assume we've managed to send the message (It's # highly unlikley we will be interupted exactly between poping a # Dump and sending the message). Wait for the confirmation before # finishing while True: with dumped_lock: if DUMPED: DUMPED = False break elif isinstance(instruction, Rotate): # Rotate follows the same theory as move while also dealing with # whether we were turning left or right if instruction.angle <= 180: angle = instruction.angle - get_odometry(rotating=True) if angle <= 20: final = [Rotate(20, 100)] else: final = [Rotate(angle, 50)] else: angle = instruction.angle + get_odometry(rotating=True) if angle >= 340: final = [Rotate(340, 100)] else: final = [Rotate(angle, 50)] elif isinstance(instruction, FromDesk): # Same as move and rotate final = [FromDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True), 50)] elif isinstance(instruction, ToDesk): # Here also dispense the letter given we are at a desk final = [ToDesk(instruction.is_left, instruction.angle - get_odometry(rotating=True)), chosen_path.pop(0), chosen_path.pop(0)] # atm it dispenses the letter even after recall # Save the generated command with final_cmd_lock: global FINAL_CMD FINAL_CMD = final # Save the current path (Will be used for resume) with chosen_path_lock: global CHOSEN_PATH CHOSEN_PATH = chosen_path # Store the node we are going to reach next (Will be used for callback) with next_node_lock: for instructione in chosen_path: if isinstance(instructione, Report): NEXT_NODE = instructione.where break sys.exit()
if left_result and right_result: left_touched = left_result[0] right_touched = right_result[0] if left_touched and right_touched: print "front" interface.setMotorPwm(motors[0],0) interface.setMotorPwm(motors[1],0) # while not interface.motorRotationSpeedReferenceReached(0): # time.sleep(0.1) time.sleep(1) interface.motorDisable(0) interface.motorDisable(3) interface.motorEnable(0) interface.motorEnable(3) time.sleep(1) print("stopped") forward(-10) time.sleep(0.5) left(90) time.sleep(0.5) elif left_touched: print "left" elif right_touched: print "right" interface.terminate()
#!/usr/bin/env python3 import move import Directions move.forward(500).join() #move.rotate(360, Directions.ROT_LEFT)
line_sensor.wait_for_line(seek_time) if line_sensor.value < 0.5: ret = True break else: direction = not direction if direction: seek_count += 1 continue return ret parser.add_option("-s", "--speed", type="float", dest="speed") (options, args) = parser.parse_args() if options.speed != None: speed = options.speed line_sensor.when_line = lambda: move.forward() line_sensor.when_no_line = lambda: move.stop() signal.signal(signal.SIGINT, exit_gracefully) while True: move.forward(speed) line_sensor.wait_for_no_line() if not seek_line(): print("Can't find any line") break exit_gracefully()
#!/usr/bin/env python3 import move import signal from gpiozero import DistanceSensor min_distance = 0.15 sonic_sensor = DistanceSensor( echo=18, trigger=17, threshold_distance=min_distance) speed = 0.5 def exit_gracefully(): exit() sonic_sensor.when_in_range = lambda: move.stop() sonic_sensor.when_out_of_range = lambda: move.forward() signal.signal(signal.SIGINT, exit_gracefully) move.forward(speed) while True: sonic_sensor.wait_for_in_range() move.backward(speed) sonic_sensor.wait_for_out_of_range() move.right(speed, 0.75) exit_gracefully()