def __init__(self, environment, address, robot_id, known, file_name=False):
self.environment = environment
self.state_est = E160_state()
self.state_est.set_state(0, 0, 0)
self.state_des = E160_state()
self.state_des.set_state(0, 0, 0)
self.state_draw = E160_state()
self.state_draw.set_state(0, 0, 0)
self.state_odo = E160_state()
self.state_odo.set_state(0, 0, 0) # real position for simulation
#self.v = 0.05
#self.w = 0.1
self.R = 0
self.L = 0
self.radius = 0.147 / 2
self.width = 2 * self.radius
self.wheel_radius = 0.03
self.address = address
self.ID = self.address.encode().__str__()[-1]
self.last_measurements = []
self.robot_id = robot_id
self.manual_control_left_motor = 0
self.manual_control_right_motor = 0
if file_name == False:
file_name = 'Log/Bot' + str(
self.robot_id) + '_' + datetime.datetime.now().replace(
microsecond=0).strftime('%y-%m-%d %H.%M.%S') + '.txt'
self.file_name = file_name
self.make_headers(self.file_name)
self.encoder_resolution = 1440
self.last_encoder_measurements = [0, 0]
self.encoder_measurements = [0, 0]
self.range_measurements = [0, 0, 0]
self.last_simulated_encoder_R = 0
self.last_simulated_encoder_L = 0
self.Kpho = 2.0 #1.0
self.Kalpha = 3.0 #2.0
self.Kbeta = -1.0 #-0.5
self.Kp = 2.0
self.max_velocity = 0.05
self.point_tracked = True
self.encoder_per_sec_to_rad_per_sec = 10
self.epsilon = 0.01
self.epsilon2 = 0.1
self.error = 0.08
self.min_rotation = 0.05
self.max_rotation = 2
self.point_reached = False
self.totalT = 0
self.PF = E160_PF(environment, self.width, self.wheel_radius,
self.encoder_resolution, known)
def __init__(self, environment, address, robot_id):
self.environment = environment
self.state_est = E160_state()
self.state_est.set_state(0, 0, 0)
self.state_des = E160_state()
self.state_des.set_state(0, 0, 0)
self.state_draw = E160_state()
self.state_draw.set_state(0, 0, 0)
self.state_odo = E160_state()
self.state_odo.set_state(0, 0, 0) # real position for simulation
#self.v = 0.05
#self.w = 0.1
self.R = 0
self.L = 0
self.radius = 0.147 / 2
self.width = 2 * self.radius
self.wheel_radius = 0.03
self.botDiameter = 0.15
self.address = address
self.ID = self.address.encode().__str__()[-1]
self.last_measurements = []
self.robot_id = robot_id
self.manual_control_left_motor = 0
self.manual_control_right_motor = 0
self.file_name = 'Log/Bot' + str(
self.robot_id
) + '_SimulationRandomStartManual_' + datetime.datetime.now().replace(
microsecond=0).strftime('%y-%m-%d %H.%M.%S') + '.txt'
self.make_headers()
#variables for logging data
self.startTime = time.time()
self.loggingData = [0, 0]
self.encoder_resolution = 1440
# random
self.last_encoder_measurements = [0, 0]
self.encoder_measurements = [0, 0]
self.range_measurements = [0, 0, 0]
self.last_simulated_encoder_R = 0
self.last_simulated_encoder_L = 0
self.Krho = 1 #1.0
self.Kalpha = 2 #2.0
self.Kbeta = -0.5 #-0.5
self.KalphaTheta = 2.0 #2.0
self.KbetaTheta = 1.5 #-0.5
self.max_velocity = 0.05
self.point_tracked = True
self.encoder_per_sec_to_rad_per_sec = 10
self.stepCount = 0
self.PF = E160_PF(environment, self.width, self.wheel_radius,
self.encoder_resolution)
def __init__(self, mode="SIMULATION MODE"):
#self.width = 2.0
#self.height = 1.2
self.width = 6.0
self.height = 3.0
# set up walls, putting top left point first
self.walls = []
#self.walls.append(E160_wall([-0.5, 0.3, -0.5, -0.3], "vertical"))
#self.walls.append(E160_wall([1, 0.8, 1, -0.3], "vertical"))
self.walls.append(E160_wall([0, 1, 15, 1], "horizontal"))
self.walls.append(E160_wall([0, -1, 15, -1], "horizontal"))
#self.walls.append(E160_wall([-1, 0.8, -1, -0.3], "vertical"))
# create vars for hardware vs simulation
# "SIMULATION MODE" or "HARDWARE MODE"
self.robot_mode = mode
self.using_leapfrog = True
self.control_mode = "MANUAL CONTROL MODE"
# setup xbee communication
if self.robot_mode == "HARDWARE MODE":
self.serial = serial.Serial('COM9', 9600)
print("Setting up serial port")
try:
self.xbee = XBee(self.serial)
print("Found Computer XBee")
except:
print("Couldn't find the serial port")
#serial_port2 = serial.Serial('COM5', 9600)
#print(" Setting up serial port")
#try:
# self.xbee2 = XBee(serial_port2)
#except:
# print("Couldn't find the serial port")
# Setup the robots
self.file_name = 'Log/{}_All_Bots' '_' \
+ datetime.datetime.now() \
.replace(microsecond=0) \
.strftime('%y-%m-%d %H.%M.%S') + '.txt'
self.file_name = self.file_name.format(self.robot_mode)
self.num_robots = 2
if self.num_robots == 1:
self.robot_pos = [(0, 0, 0)]
elif self.num_robots == 2:
self.robot_pos = [(0.5, 0, 0), (0, 0, 0)]
self.robots = []
self.state_odo = [E160_state() for _ in range(self.num_robots)]
addresses = ['\x00\x0C', '\x00\x01']
for i in range(self.num_robots):
# TODO: assign different address to each bot
r = E160_robot(self, addresses[i], i)
self.robots.append(r)
self.state_odo[i].set_from_tuple(self.robot_pos[i])
# Pair the two robots up
if self.num_robots == 2:
self.robots[0].other_pair_id = 1
self.robots[1].other_pair_id = 0
# Store the measurements of all robots here.
self.range_meas = [[0] for _ in self.robots]
self.encoder_meas = [[0, 0] for _ in self.robots]
self.last_encoder_meas = [[0, 0] for _ in self.robots]
self.bearing_from_other = [0 for _ in self.robots]
self.pf = E160_PF.E160_PF(self, self.robots)
self.make_header(self.num_robots)
def __init__(self, environment, address, robot_id):
self.environment = environment
self.state_est = E160_state()
self.state_est.set_state(0, 0, 0)
self.state_des = E160_state()
self.state_des.set_state(0, 0, 0)
self.state_draw = E160_state()
self.state_draw.set_state(0, 0, 0)
self.state_odo = E160_state()
self.state_odo.set_state(0, 0, 0) # real position for simulation
self.state_error = E160_state()
self.state_curr_dest = E160_state()
self.state_curr_dest.set_state(0.0, 0.0, 0.0)
#self.v = 0.05
#self.w = 0.1
self.R = 0
self.L = 0
self.radius = 0.147 / 2
self.width = 2 * self.radius
self.wheel_radius = 0.03
self.botDiameter = 0.15
self.address = address
self.ID = self.address.encode().__str__()[-1]
self.last_measurements = []
self.robot_id = robot_id
self.manual_control_left_motor = 0
self.manual_control_right_motor = 0
self.file_name = 'Log/Bot' + str(
self.robot_id) + '_' + datetime.datetime.now().replace(
microsecond=0).strftime('%y-%m-%d %H.%M.%S') + '.txt'
self.make_headers()
self.encoder_resolution = 1440
self.last_encoder_measurements = [0, 0]
self.encoder_measurements = [0, 0]
self.range_measurements = [0, 0, 0]
self.last_simulated_encoder_R = 0
self.last_simulated_encoder_L = 0
self.Kpho = 1 #1.0
self.Kalpha = 2 #2.0
self.Kbeta = -0.5 #-0.5
self.KalphaTheta = 2.0 #2.0
self.KbetaTheta = 1.5 #-0.5
self.max_velocity = 0.05
self.point_tracked = True
self.encoder_per_sec_to_rad_per_sec = 10
self.min_ptrack_dist_error = 0.1
self.min_ptrack_ang_error = math.pi
self.max_ang_velocity = 0.5
self.trajectory = []
self.path_counter = 0
# Path Planner and Particle Filter
self.PF = E160_PF(environment, self.width, self.wheel_radius,
self.encoder_resolution)
self.MP = E160_MP(environment, self.state_odo, self.radius)
self.build_path([0], self.MP.node_list)
self.replan_path = False
def __init__(self, environment, address, robot_id):
self.environment = environment
# estimated state
self.state_est = E160_state()
self.state_est.set_state(0, 0, 0)
self.state_est_PF = self.state_est.copy()
self.state_est_UKF = self.state_est.copy()
self.state_est_prev = E160_state()
self.state_est_prev.set_state(0, 0, 0)
# desired state
self.state_des = E160_state()
self.state_des.set_state(3.5, 0.75, 0)
# state error (des-est)
self.state_error = E160_state()
self.state_error.set_state(0, 0, 0)
self.state_draw = E160_state()
self.state_draw.set_state(0, 0, 0)
self.state_odo = E160_state()
self.state_odo.set_state(0.875 / 2, 3.1,
-math.pi / 2) # real position for simulation
# self.state_odo.set_state(0,0,0) # real position for simulation
self.state_curr_dest = E160_state()
self.state_curr_dest.set_state(0.0, 0.0, 0.0)
# dimension of robot state space
self.DIM = 3
# delete?
self.previous_state_error = []
self.R = 0
self.L = 0
self.v = 0.05
self.w = 0.1
# self.radius = 0.12
# self.wheel_radius = 0.03
self.radius = 0.14149 / 2
self.width = 2 * self.radius
self.wheel_radius = 0.06955 / 2
self.sl = 0
self.sr = 0
self.address = address
self.ID = self.address.encode().__str__()[-1]
self.MAX_PAST_MEASUREMENTS = 10
self.last_measurements = []
self.last_move_forward_measurements = []
self.robot_id = robot_id
self.manual_control_left_motor = 0
self.manual_control_right_motor = 0
date = datetime.datetime.now().replace(microsecond=0).__str__()
date = date.replace(" ", "_").replace("-", "_").replace(":", "_")
self.file_name = 'Log/Bot' + str(self.robot_id) + '_' + date
self.file_name = self.file_name.replace(" ", "")
self.make_headers()
self.encoder_resolution = 1440
self.last_encoder_measurements = [0, 0]
self.encoder_measurements = [0, 0]
self.range_measurements = [float('inf'), float('inf'), float('inf')]
self.last_simulated_encoder_R = 0
self.last_simulated_encoder_L = 0
# distance sensor offsets from center of robot [front, right, left]
self.sensor_offset = [0.076, 0.076, 0.076]
self.front_dist = 0
self.left_dist = 0
self.right_dist = 0
self.control_effort = 0
self.error = 0
self.move_forward_error = 0
self.time_step = 0.1
self.start = True
self.last_simulated_encoder_R = 0
self.last_simulated_encoder_L = 0
self.min_ptrack_dist_error = 0.05 # meters
self.min_ptrack_ang_error = 0.05 # radians
if self.environment.robot_mode == "SIMULATION MODE":
#simulation gains
self.Kpho = 1.4 #1.0
self.Kalpha = 1.5 #2.5
self.Kbeta = -1.2 #-0.5
self.max_velocity = 0.05
self.max_ang_velocity = 0.5
self.min_ptrack_dist_error = 0.05 # meters
self.min_ptrack_ang_error = 0.05 # radians
else:
# hardware gains
self.Kpho = 1.0 #1.0
self.Kalpha = 2.9 #2.0
self.Kbeta = -2.5 #-2.5
self.max_velocity = 0.1
self.max_ang_velocity = 0.8
self.min_ptrack_dist_error = 0.05 # meters
self.min_ptrack_ang_error = 0.05 # radians
self.point_tracked = True
self.encoder_per_sec_to_rad_per_sec = 10
self.path_counter = 0
self.path = [
E160_state(0.4375, 0.355, -math.pi / 2),
E160_state(0.4375, 0.355, 0),
E160_state(3, 0.355, 0),
E160_state(3, 0.355, math.pi / 2),
E160_state(3, 1.065, math.pi / 2),
E160_state(3, 1.065, math.pi),
E160_state(1.3125, 1.065, math.pi),
E160_state(1.3125, 1.065, math.pi / 2),
E160_state(1.3125, 3.1, math.pi / 2),
E160_state(1.3125, 3.1, math.pi),
E160_state(0.4375, 3.1, math.pi),
E160_state(0.4375, 3.1, -math.pi / 2)
]
# self.path = [E160_state(0.875, 0.71, -math.pi/2), E160_state(0.875, 0.71, 0),
# E160_state(3, 0.71, 0), E160_state(3, 0.71, -math.pi),
# E160_state(0.875, 0.71, -math.pi), E160_state(0.875, 0.71, math.pi/2),
# E160_state(1.75/2,3, math.pi/2), E160_state(0.875, 0.71, -math.pi)]
# self.path = [E160_state(0.25, -0.25, 0), E160_state(0.25, -0.25, math.pi/2),
# E160_state(0.25, 0.25, math.pi/2), E160_state(0.25, 0.25, math.pi),
# E160_state(-0.75, 0.25, math.pi), E160_state(-0.75, 0.25, 0),
# E160_state(0.25, 0.25, 0), E160_state(0.25, 0.25, math.pi/2),
# E160_state(0.25, -0.25, math.pi/2), E160_state(0.25, -0.25, 0),
# E160_state(-0.75, -0.25, 0)]
# self.path = [E160_state(0.5, 0, 1.57), E160_state(0.5, 0.5, 3.14),
# E160_state(0, 0.5, 3.14+1.57), E160_state(0, 0, 0),
# E160_state(0.25, 0.25, 1.57), E160_state(0, 0, 0),
# E160_state(0.25, 0, 1.57), E160_state(0, 0, 0),
# E160_state(0, 0.25, 0), E160_state(0, 0, 0),
# E160_state(0, 0.25, -1.57), E160_state(0, 0, 0),
# E160_state(-0.25, 0, 3.14), E160_state(0, 0, 0),
# E160_state(0.25, 0, 3.14), E160_state(0, 0, 0)]
# self.path = [E160_state(0, 0, 1.57), E160_state(0, 0, 0),
# E160_state(0, 0, -2), E160_state(0, 0, 2),
# E160_state(0, 0, 3.14), E160_state(0, 0, 0),
# E160_state(0.5, 0, 1.57), E160_state(0.5, 0.5, 3.14),
# E160_state(0, 0.5, 3.14+1.57), E160_state(0, 0, 0),
# E160_state(0.25, 0.25, 1.57), E160_state(0, 0, 0),
# E160_state(0.25, 0, 1.57), E160_state(0, 0, 0),
# E160_state(0, 0.25, 0), E160_state(0, 0, 0),
# E160_state(0, 0.25, -1.57), E160_state(0, 0, 0),
# E160_state(-0.25, 0, 3.14), E160_state(0, 0, 0),
# E160_state(0.25, 0, 3.14), E160_state(0, 0, 0)]
# self.path = [E160_state(0, 0, 0.2), E160_state(0, 0, -0.2), E160_state(0, 0, 0),
# E160_state(0, 0, 1.57), E160_state(0, 0, -1.57), E160_state(0, 0, 0),
# E160_state(0, 0, 3.14), E160_state(0, 0, 0), E160_state(0, 0, -3.14), E160_state(0, 0, 2), E160_state(0, 0, -2)]
state_offset = [0, 0, 0]
self.state_filter = E160_state()
self.state_filter.set_state(self.state_odo.x + state_offset[0],
self.state_odo.y + state_offset[1],
self.state_odo.theta + state_offset[2])
# create filter
self.PF = E160_PF(environment,
self.width,
self.wheel_radius,
self.encoder_resolution,
initialState=self.state_filter.copy())
self.UKF = E160_UKF(environment,
self.DIM,
self.width,
self.wheel_radius,
self.encoder_resolution,
initialState=self.state_filter.copy())
self.AUKF = E160_AUKF(environment,
self.DIM,
self.width,
self.wheel_radius,
self.encoder_resolution,
initialState=self.state_filter.copy())
self.trajectory = []
self.path_counter = 0
# add motion planner
self.MP = E160_MP(environment, self.state_odo, self.radius)
self.build_path([0], self.MP.node_list)
self.replan_path = True
# simluation noise parameters
self.s_std = 0.001
self.theta_std = 0.05
self.sensor_std = 0.05