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)
