def decide_next_action(pose, distace_tolerance, heading_torelance):
# decide the next action from current robot status and the next waypoint
current_point = np.array([pose[1], pose[0]])
current_yaw = pose[2]
diff_distance = round(
mpu.haversine_distance(current_point, BIWAKO.next_goal), 5) * 1000
e_dis.append(diff_distance)
# check distance between current and target
if abs(diff_distance) < distace_tolerance:
ch = 4
pwm = 1500
action = [ch, pwm]
print("achieve the target point")
print("########################")
print("########################")
if BIWAKO.way_point_num != -1:
BIWAKO.update_next_goal()
print("change waypoint")
print("next way point: ", BIWAKO.next_goal)
elif BIWAKO.way_point_num == -1:
ch = 4
pwm = 1500
action = [ch, pwm]
print("Mission complete")
else:
ch = 4
pwm = 1500
action = [ch, pwm]
print("Way point error")
return action
# when the device has not received
else:
target_direction = math.radians(
calculator.calculate_bearing(current_point, BIWAKO.next_goal))
diff_deg = math.degrees(
calculator.limit_angle(target_direction - current_yaw))
e_deg.append(diff_deg)
if abs(diff_deg) < heading_torelance:
ch = 5
pwm = PD_control_dis(diff_distance)
print("Straight")
elif diff_deg >= heading_torelance:
ch = 4
pwm = PD_control_deg(diff_deg)
print("Turn right")
elif diff_deg < -1.0 * heading_torelance:
ch = 4
pwm = PD_control_deg(diff_deg)
print("Turn left")
action = [ch, pwm]
print("diff: ", diff_deg)
return action
def decide_next_action_omni(pose, distace_tolerance, heading_torelance):
# decide the next action from current robot status and the next waypoint
current_point = np.array([pose[1], pose[0]])
current_yaw = pose[2]
diff_distance = round(
mpu.haversine_distance(current_point, BIWAKO.next_goal), 5) * 1000
e_dis.append(diff_distance)
# check distance between current and target
if abs(diff_distance) < distace_tolerance:
ch = 4
pwm = 1500
action = [ch, pwm]
print("Achive the point")
print("########################")
return action
# when the device has not received
else:
target_direction = math.radians(
calculator.calculate_bearing(current_point, BIWAKO.next_goal))
diff_deg = math.degrees(
calculator.limit_angle(target_direction - current_yaw))
e_deg.append(diff_deg)
pwm = P_control(diff_distance)
if -45.0 <= diff_deg < 45:
ch = 5
print("Forward")
elif -180.0 <= diff_deg < -135.0 or 135.0 <= diff_deg < 180.0:
ch = 5
pwm = 3000 - pwm
print("Backward")
elif 45.0 <= diff_deg < 135.0:
ch = 6
print("Right")
elif -135.0 <= diff_deg < -45.0:
ch = 6
pwm = 3000 - pwm
print("Left")
action = [ch, pwm]
print("pwm: ", pwm)
print("diff deg: ", diff_deg)
print("diff distance: ", diff_distance)
return action
def decide_next_action(pose, start_time, distace_tolerance, heading_torelance):
# decide the next action from current robot status and the next waypoint
current_point = np.array([pose[1], pose[0]])
current_yaw = pose[2]
diff_distance = round(
mpu.haversine_distance(current_point, BIWAKO.next_goal), 5) * 1000
e_dis.append(diff_distance)
# check distance between current and target
if abs(diff_distance) < distace_tolerance:
ch = 4
pwm = 1500
action = [ch, pwm]
isKeep_position = True
if start_time == 0.0:
start_time = time.time()
print("Keep the position")
print("########################")
return action
# when the device has not received
else:
isKeep_position = True
target_direction = math.radians(
calculator.calculate_bearing(current_point, BIWAKO.next_goal))
diff_deg = math.degrees(
calculator.limit_angle(target_direction - current_yaw))
e_deg.append(diff_deg)
if abs(diff_deg) < heading_torelance:
ch = 5
pwm = PD_control_dis(diff_distance)
print("Straight")
elif diff_deg >= heading_torelance:
ch = 4
pwm = PD_control_deg(diff_deg)
print("Turn right")
elif diff_deg < -1.0 * heading_torelance:
ch = 4
pwm = PD_control_deg(diff_deg)
print("Turn left")
action = [ch, pwm]
print("pwm: ", pwm)
print("diff: ", diff_deg)
return action
def main(strategy, disturbance_mode, gps_error_mode, filename):
a_gps = robot.getDevice("a_gps")
a_gps.enable(gps_sampling_rate)
e_gps = robot.getDevice("e_gps")
e_gps.enable(gps_sampling_rate)
compass = robot.getDevice("compass")
compass.enable(compass_sampling_rate)
# simulation mode parameters
control_mode = strategy[0]
policy = strategy[1]
distance_torelance = strategy[2][0]
main_distance_tolerance = strategy[2][0]
temp_distance_torelance = strategy[2][1]
total_step = parameter.total_step
display_mode = parameter.state_display_mode
# log parameter lists
diff_distance = [0.0]
t_diff_distance = [0.0]
a_diff_distance = [0.0]
diff_deg = [0.0]
# electlicity parameters
V = parameter.V
P = 0.0
# localizaation parameters
next_goal = target_point[0]
current_point = [35.0494, 135.924]
# disturbance parameters
d_count = 0
x_list = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3]
z_list = [0.05, 0.1, 0.15, 0.2, 0.25, 0.3]
# control flags
count = 0.0
temp_flag = 0
is_First = 0
current_flag = 1
if parameter.data_log_mode == True:
param_file = workspace + str_date + "/" + filename + ".txt"
csv_filename = workspace + str_date + "/" + str_date + "_" + filename + ".csv"
f = open(csv_filename, 'a', newline='')
log_param_data(param_file)
csvWriter = csv.writer(f)
csvWriter.writerow([
'count', 'a_latitude', 'a_longitude', 'e_latitude', 'e_longitude',
'W', 'P'
])
while robot.step(TIME_STEP) != -1:
a_gps_value = a_gps.getValues()
e_gps_value = e_gps.getValues()
compass_value = compass.getValues()
bearing = math.radians(calculator.get_bearing_in_degree(compass_value))
a_latitude = a_gps_value[1]
a_longitude = a_gps_value[0]
e_latitude = e_gps_value[1]
e_longitude = e_gps_value[0]
a_current_point = [a_latitude, a_longitude]
e_current_point = [e_latitude, e_longitude]
if gps_error_mode == True:
current_point = e_current_point
else:
current_point = a_current_point
target_direction = math.radians(
calculator.calculate_bearing(current_point, next_goal))
deg = math.degrees(calculator.limit_angle(target_direction - bearing))
diff_deg.append(deg)
distance = round(mpu.haversine_distance(current_point, next_goal),
5) * 1000
t_distance = round(
mpu.haversine_distance(current_point, target_point[0]), 5) * 1000
a_distance = round(
mpu.haversine_distance(a_current_point, target_point[0]), 5) * 1000
diff_distance.append(distance)
t_diff_distance.append(t_distance)
a_diff_distance.append(a_distance)
if diff_distance[-1] <= distance_torelance:
current_First = 0
if policy == 1:
if is_First == 0:
is_First = 1
temp_goal = target_point[0]
temp_flag = 0
next_goal = target_point[0]
distance_torelance = main_distance_tolerance
if debug_mode == True:
print("STAY")
thruster_direction = [0, 0, 0, 0]
thrust = 0.0
if diff_distance[-1] > distance_torelance:
if policy == 0:
pass
elif policy == 1 and temp_flag == 0 and is_First == 1:
temp_goal = calculator.calc_flexible_temp_goal(current_point,
target_point[0],
temp_goal,
r=3.0)
next_goal = temp_goal
distance_torelance = temp_distance_torelance
distance = round(
mpu.haversine_distance(current_point, next_goal), 5) * 1000
temp_flag = 1
current_flag = 1
if control_mode == 0:
thruster_direction, thrust = controller.push_omni_control_action(
diff_distance, diff_deg)
elif control_mode == 1:
thruster_direction, thrust = controller.diagonal_control_action(
diff_distance, diff_deg)
elif control_mode == 2:
thruster_direction, thrust = controller.fixed_head_action(
diff_distance, diff_deg)
elif control_mode == 3:
thruster_direction, thrust = controller.oct_directional_action(
diff_distance, diff_deg)
set_thruster_vel(thruster_direction, thrust)
# calculate E-energy
A = calculator.calc_amp(thrust, thruster_direction)
# if the thruster state shift from off to run, the current is employed the peak value
if diff_distance[-2] < main_distance_tolerance and diff_distance[
-1] > distance_torelance and current_flag == 1:
A = calculator.calc_peak_amp(thrust, thruster_direction)
current_flag = 0
W = calculator.calc_Watt(V, A, TIME_STEP)
P = P + W / 1000 # 消費電力グラフの単位を[kJ]としているため1000で割る
if count % (total_step / 5) == 0:
if disturbance_mode == 0:
x = 0.1
z = 0
elif disturbance_mode == 1:
x = round(random.uniform(-0.3, 0.3), 2)
z = round(random.uniform(-0.3, 0.3), 2)
elif disturbance_mode == 2:
x = x_list[d_count]
z = 0.0
d_count = d_count + 1
set_disturbance(x, z)
count = count + 1
if count == total_step + 1:
if parameter.data_log_mode == True:
m_a_diff_distance = mean(a_diff_distance)
m_t_diff_distance = mean(t_diff_distance)
m_diff_distance = mean(diff_distance)
path = workspace + str_date + "/" + filename + "_distance.txt"
make_distance_log_file(path, m_a_diff_distance,
m_t_diff_distance, m_diff_distance, P)
print("File close")
f.close()
x = 0.0
z = 0.0
count = 0
break
if parameter.data_log_mode == True:
csvWriter.writerow([
count, a_latitude, a_longitude, e_latitude, e_longitude, W, P
])
if display_mode:
power_label = "Comsumed energy: " + str(
'{:.2f}'.format(P)) + " [Ws]"
robot.setLabel(4, power_label, 0.5, 0.4, 0.1, 0x00FF00, 0, "Arial")
while abs(diff_distance) > temp_target_distance_torelance:
pose = [BIWAKO.lat, BIWAKO.lon, BIWAKO.yaw]
# decide the next action from current robot status and the next waypoint
current_point = np.array([pose[0], pose[1]])
current_yaw = pose[2]
# location input format: [latitude, longotude]
diff_distance = round(
mpu.haversine_distance(current_point,
BIWAKO.temp_goal), 5) * 1000
target_direction = math.radians(
calculator.calculate_bearing(current_point,
BIWAKO.temp_goal))
diff_deg = math.degrees(
calculator.limit_angle(target_direction - current_yaw))
deg_e.append(diff_deg)
if control_mode == 0:
action = actions.omni_control_action(
diff_deg, diff_distance)
action_log.append(action)
BIWAKO.cmd = action[0]
BIWAKO.pwm = action[1]
control_thruster(action, action_log, thruster_control)
time.sleep(0.01)
elif control_mode == 1:
action = actions.diagonal_action(
diff_deg, diff_distance, deg_e)
control_thruster(action[0])
control_thruster(action[1])