def line_follower(self, plan): ''' Takes a plan as a PoseArray and processes the plan into a 3 element numpy array (x,y,theta) then a LineFollower object is created and the plan is passed ''' # set values for LineFollower object plan_lookahead = 5 translation_weight = 1.0 rotation_weight = 1.0 kp = 0.7 ki = 0.0 kd = 0.0 error_buff_length = 10 speed = 1.0 raw_input( "Press Enter when plan available...") # Waits for ENTER key press print('got plan') poses = plan.poses # Convert the plan PoseArray to a list of 3-element numpy arrays # Each array is of the form [x,y,theta] pos_list = [] for i in range(len(poses)): temp_array = np.array([poses[i].position.x,\ poses[i].position.y,\ Utils.quaternion_to_angle(poses[i].orientation)]) pos_list.append(temp_array) # Create a LineFollower object line_follower = LineFollower(pos_list,pose_topic,plan_lookahead,translation_weight,rotation_weight,\ kp,ki,kd,error_buff_length,speed)
async def on_connect(socket, path): try: movesteering = MoveSteering(OUTPUT_A, OUTPUT_B) colorsensor = ColorSensor(INPUT_1) fork = MediumMotor(OUTPUT_C) remote_control = RemoteControl(socket, movesteering, fork) fixed_mode = FixedMode(socket, movesteering) line_follower = LineFollower(movesteering, colorsensor) mode = remote_control #mode = fixed_mode # while True: # raw_cmd = "" while True: try: raw_cmd = await asyncio.wait_for(socket.recv(), timeout = 500) #await mode.run(raw_cmd) mode.run(raw_cmd) except TimeoutError: pass # if raw_cmd != "": # await mode.run(raw_cmd) #else: # print("CHANGING MODE") # print(raw_cmd) # # command = json.loads(raw_cmd) # command_type = command['type'] # if command_type == 'MODE': # old_number = mode_number # mode_number = command['mode'] # # print(mode_number) # print(old_number) # # if mode_number != old_number: # mode.stop() # if mode_number == 1: # mode = line_follower # mode.start() # elif mode_number == 2: # print("2") # elif mode_number == 3: # print("3") # elif mode_number == 4: # print("4") # elif mode_number == 5: # print("5") # elif mode_number == 6: # mode = remote_control # start repl # mode.start() except ConnectionClosed: pass
def change_mode(self, mode, args=None): self.mode = mode print("Changing mode...") if self.mode == 'Line Follower': print("Creating Line Follower...") self.thread = LineFollower(self.change_mode) self.thread.start() elif self.mode == 'Manual Control': print("Starting manual control...") self.thread = ManualControl(args) print("Manual control started") elif self.mode == 'Forest Crawler': print("Creating Forest Crawler...") self.thread = ForestCrawler(self.change_mode) self.thread.start() elif self.mode == 'Cube Carrier': print("Creating Cube Carrier...") self.thread = CubeCarrier(self.change_mode) self.thread.start() elif self.mode == 'Disc Traveler': print("Creating Disc Traveler...") self.thread = DiscTraveler(self.change_mode) self.thread.start() elif self.mode == 'Slope Searcher': print("Creating Slope Searcher...") self.thread = SlopeSearcher(self.change_mode) self.thread.start() elif self.mode == 'Battle Mode': print("Creating Battle Mode...") self.thread = BattleMode(self.change_mode) print("FOOBAR") self.thread.start() elif self.mode == 'Pause': print("Pausing...") self.thread = Pause() self.thread.start() elif self.mode == 'Stop': print("Stop")
import time import client_socket from robot import Robot from line_follower import LineFollower from environment import Environment robot = Robot() env = Environment() lf = LineFollower(robot) # right angle def turn_right(slow_end=False): robot.rotate_right_until_detected(slow_end=slow_end) robot.stop() #robot.rotate_by_degree(degrees = 85) # right angle def turn_left(slow_end=False): robot.rotate_left_until_detected(slow_end=slow_end) robot.stop() #robot.rotate_by_degree(degrees = -85) # follows white line until an intersection is discovered (to be replaced by pi vision) #overrun = do overrun after intersection #fast = line following speed increase def follow_line_until_intersection(overrun=False, overrun_short=False, fast=False):
controller_params.kp = 50.0 controller_params.ki = 0.0 controller_params.kd = 3.0 # Defining robot parameters robot_params = Params() robot_params.sensor_offset = 0.05 robot_params.max_wheel_speed = 45.0 robot_params.wheel_radius = 0.02 robot_params.wheels_distance = 0.05 robot_params.wheel_bandwidth = 10.0 * 2.0 * pi # Defining line sensor parameters sensor_params = Params() sensor_params.sensor_range = 0.015 sensor_params.num_sensors = 7 sensor_params.array_width = 0.06 line_follower = LineFollower(Pose(0.5, 0.5, 45.0 * pi / 180.0), controller_params, robot_params, sensor_params) # Defining PSO hyperparameters hyperparams = Params() hyperparams.num_particles = 40 hyperparams.inertia_weight = 0.5 hyperparams.cognitive_parameter = 0.6 hyperparams.social_parameter = 0.8 lower_bound = np.array([0.0, 10.0, 0.0, 0.0]) upper_bound = np.array([0.9, 200.0, 1300.0, 30.0]) pso = ParticleSwarmOptimization(hyperparams, lower_bound, upper_bound) # Creating track # Switch to simple track if you are having trouble to make the robot learn in the complex track track = create_complex_track() # create_simple_track()
# Defining robot parameters robot_params = Params() robot_params.sensor_offset = 0.05 robot_params.max_wheel_speed = 45.0 robot_params.wheel_radius = 0.02 robot_params.wheels_distance = 0.05 robot_params.wheel_bandwidth = 10.0 * 2.0 * pi # Defining line sensor parameters sensor_params = Params() sensor_params.sensor_range = 0.015 sensor_params.num_sensors = 7 sensor_params.array_width = 0.06 # Defining controller params (including learning algorithm) linear_speed = 0.7 rl_algorithm = configure_rl_algorithm() line_follower = LineFollower(Pose(0.0, 0.0, 0.0), rl_algorithm, linear_speed, robot_params, sensor_params) # Creating track # Switch to simple track if you are having trouble to make the robot learn in the complex track track = create_complex_track() # create_simple_track() # Creating the simulation simulation = Simulation(line_follower, track) # Initializing pygame pygame.init() window = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT)) pygame.display.set_caption("Lab 12 - Model-Free RL Algorithms") clock = pygame.time.Clock() font = pygame.font.SysFont('Arial', 20, True)
def setUp(self): self.linefollower = LineFollower()
class LineFollowingTestCase(unittest.TestCase): def setUp(self): self.linefollower = LineFollower() def tearDown(self): self.linefollower = None def test_1_straight(self): # Initial conditions x_0 = 0 y_0 = 5 theta_0 = -3.0 * np.pi / 4.0 X_0 = [x_0, y_0, theta_0] # Initial state # Line equation: ax + by + c = 0 a, b, c = 1.0, 5.0, 4.0 line = [a, b, c] # Vehicle's trajectory traj_x, traj_y, traj_theta = self.linefollower.move2pose( X_0, line, params) TrackedPathPlotter.path_visualizer(traj_x, traj_y, X_0, line) def test_2_turn(self): # Initial conditions x_0 = 8 y_0 = 4 theta_0 = -3.0 * np.pi / 4.0 X_0 = [x_0, y_0, theta_0] # Initial state # Line equation: ax + by + c = 0 a, b, c = 1.0, -2.0, 4.0 line = [a, b, c] # Vehicle's trajectory traj_x, traj_y, traj_theta = self.linefollower.move2pose( X_0, line, params) TrackedPathPlotter.path_visualizer(traj_x, traj_y, X_0, line) def test_3_random(self): for i in range(nb_of_tests): # Initial conditions x_0 = 6 * random() - 3 y_0 = 6 * random() - 3 theta_0 = 2 * np.pi * random() - np.pi X_0 = [x_0, y_0, theta_0] # Initial state # Line equation: ax + by + c = 0 a, b, c = 1.0, -2.0, 0.0 line = [a, b, c] # Vehicle's trajectory traj_x, traj_y, traj_theta = self.linefollower.move2pose( X_0, line, params) plt.arrow(X_0[0], X_0[1], np.cos(X_0[2]), np.sin(X_0[2]), head_width=0.2, head_length=0.3, length_includes_head=True, width=0.05, color='black') plt.plot(traj_x, traj_y) x = np.linspace(-10, 10, 100) plt.plot(x, -(a / b) * x - c / b, '-b', linewidth=2, label='y=2x+1') plt.xlabel('X') plt.ylabel('Y') plt.grid(b=True, which='major', color='black', linestyle='-', linewidth=0.5) plt.grid(b=True, which='minor', color='black', linestyle='--', linewidth=0.2) plt.minorticks_on() plt.xlim([-10, 10]) plt.ylim([-10, 10]) plt.ion() plt.show() raw_input("Press Enter to close plot...") plt.close()
def line_follower_process(car, sensors): line = LineFollower(car, sensors) line.follow_line()
import json import time from car_movement import CarMovement from sensors import Sensors from line_follower import LineFollower try: car = CarMovement() sensors = Sensors() line_follower = LineFollower(car, sensors) line_follower.calibration(wheels=True) print(json.dumps({"message": "Line sensor calibrated successfully"})) except Exception, e: print("ERROR:{}".format(e))
def line_follower_process(car): line = LineFollower(car) line.follow_line()
#!/usr/bin/env pybricks-micropython from dependencies import * from line_follower import LineFollower from brick_detector import BrickDetector from planner import Planner from depo_navigator import DepoNavigator if __name__ == "__main__": print("Initializing") lf = LineFollower() bd = BrickDetector() p = Planner("SL") dn = DepoNavigator() last_mile = False skip_one_turn = False lf.left = p.left print("Started") while True: # Manual control. if Button.LEFT in brick.buttons(): motor = Motor(Port.A, Direction.CLOCKWISE) motor.run_angle(360, 90) if Button.RIGHT in brick.buttons(): motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) motor.run_angle(360, 90)