def waypoint_manager(self):
if self._distance < 2:
if self._curr_waypoint_num < self._waypoints_num:
try:
self._curr_waypoint_num += 1
self._next_waypoint = self._waypoints[
self._curr_waypoint_num]
msg = "Waypoint reached, heading to #{0} at {1}\n".format(
str(self._curr_waypoint_num + 1),
str(self._next_waypoint))
cmd.send_uart(msg, log_flag)
msg = {
"type": "gps",
"func": "next_waypoint",
"index": str(self._curr_waypoint_num + 1)
}
msg = ujson.dump(msg)
cmd.send_uart(msg, log_flag)
return True
except IndexError:
cmd.send_uart("Waypoint manager IndexError\n", log_flag)
return False
else:
cmd.send_uart("All waypoints have been covered", log_flag)
self.stop_planner()
msg = {"type": "gps", "func": "next_waypoint", "index": -1}
msg = ujson.dump(msg)
cmd.send_uart(msg, log_flag)
return False
else:
return True
def resume_planner(self):
if self._planner_init is True:
global_variables.pid_test.start_update()
cmd.send_uart("Autonomous mode resumed\n", log_flag)
else:
cmd.send_uart("Autonomous mode hasn't been started yet\n",
log_flag)
def gps_data(self):
try:
msg = {
"type": "gps",
"func": "position",
"lat": "{:.7f}".format(self.latitude),
"long": "{:.7f}".format(self.longitude),
"hdop": str(self.hdop),
"speed": "{:.2f}".format(self.speed)
}
except ValueError:
msg = {
"type": "gps",
"func": "position",
"lat": str(self.latitude),
"long": str(self.longitude),
"hdop": str(self.hdop),
"speed": str(self.speed)
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
if debug_flag is True:
cmd.debug(msg, debug_flag)
def start_update(self):
# 16-bit timer
self.timer = Timer(9)
self.timer.init(freq=100)
self.timer.callback(self.update)
self.enable = True
self.error_sum = 0
cmd.send_uart("PID timers started\n", log_flag)
def pause_planner(self):
if self._planner_init is True:
self.status = "Pause"
global_variables.pid_test.stop_update()
cmd.send_uart("Autonomous mode paused\n", log_flag)
else:
cmd.send_uart("Autonomous mode hasn't been started yet\n",
log_flag)
def waypoint_add(self, start):
if start is True:
self._waypoints.clear()
else:
cmd.send_uart(
"{0:d} waypoints added\n".format(len(self._waypoints)),
log_flag)
self._add = start
def waypoint_coord(self, waypoint):
if self._add is True:
try:
self._waypoints.append(
(float(waypoint[0]), float(waypoint[1])))
except ValueError:
cmd.send_uart("Invalid coordinate format", log_flag)
print(waypoint[0], waypoint[1])
def stop_update(self):
try:
self.timer.deinit()
except AttributeError:
cmd.send_uart("PID not initialised\n", log_flag)
self.enable = False
self.error_sum = 0
motor.motor_abs(("l", 0), ("r", 0))
cmd.send_uart("PID stopped\n", log_flag)
def stop_planner(self):
self.status = "Stop"
global_variables.pid_test.stop_update()
self._distance = None
self._curr_waypoint_num = 0
self._waypoints.clear()
self._next_waypoint = (None, None)
self._bearing = None
self._planner_init = False
cmd.send_uart("Autonomous mode stopped\n", log_flag)
def planner_data(self):
msg = {
"type": "gps",
"func": "planner",
"stat": str(self.status),
"tar_head": str(self._bearing),
"way_num": str(self._curr_waypoint_num + 1),
"next_way_lat": str(self._next_waypoint[0]),
"next_way_lon": str(self._next_waypoint[1]),
"way_dist": str(self._distance)
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
if debug_flag is True:
cmd.debug(msg, debug_flag)
def dist_bearing(self):
if ((self._gps.pos_fix > 0) or
(self._gps.status == "A")) and self._gps.latitude is not None:
self._current_pos = (float(self._gps.latitude),
float(self._gps.longitude))
try:
# A is current_pos in rad, B is next_waypoint in rad
A = ([0.0174533 * x for x in self._current_pos])
B = ([0.0174533 * x for x in self._next_waypoint])
d_long = float(B[1] - A[1])
# This is true North heading
self._bearing = math.atan2(
(math.sin(d_long) * math.cos(B[0])),
(math.cos(A[0]) * math.sin(B[0]) - math.sin(A[0]) *
math.cos(B[0]) * math.cos(d_long))) * 57.29577951
if self._bearing > 180:
self._bearing -= 360
# Distance calculated using the haversine formula
# Earth's mean radius in m
R = 6371000
d_lat = float(B[0] - A[0])
a = math.sin(d_lat / 2)**2 + math.cos(A[0]) * math.cos(
B[0]) * math.sin(d_long / 2)**2
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
self._distance = R * c
self.i = 0
# Updates waypoints if robot has reached waypoint
flag = self.waypoint_manager()
if flag:
return True, (self._distance, self._bearing)
else:
return False, (None, None)
except TypeError:
if self.i > 5:
cmd.send_uart("Inaccurate hdop in dist_bearing", log_flag)
self.pause_planner()
self.i += 1
return False, (None, None)
else:
if self.i > 5:
cmd.send_uart("Inaccurate pos in dist_bearing", log_flag)
self.pause_planner()
self.i += 1
return False, (None, None)
def init():
global uart
# Initialising UART6 (Y1, Y2)
uart = UART(6, baudrate=38400, timeout=10, read_buf_len=200)
pyb.repl_uart(uart)
print('RPi UART setup')
global uart_gps
global gps_device
global gps_planner
uart_gps = UART(4, baudrate=9600, timeout=10, read_buf_len=600)
gps_device = gps.GPS(uart_gps)
gps_planner = planner.PLANNER(gps_device)
print("GPS set up")
global pid_test
pid_test = pid.PID(0.3, 0, 0.1, gps_planner.dist_bearing, 1)
# If True, uart messages are sent back normally,
# if false, only log messages are sent back.
global UART_comms_master
UART_comms_master = False
global imu, fuse, i2c
global imu_acc, imu_gyr, imu_mag
i2c = I2C(scl=Pin('Y9'), sda=Pin('Y10'))
try:
imu = MPU9250(i2c, scaling=(-1, -1, -1))
imu._mag.cal = (20.915039062, 11.880468750, 23.293359375)
imu.gyro_filter_range = 4
imu.accel_filter_range = 4
print(imu.mag.cal)
print(imu.accel.cal)
print(imu.gyro.cal)
mpu_addr = "MPU9250 id: {:X}".format(imu.mpu_addr)
cmd.send_uart(mpu_addr, True)
fuse = Fusion()
except:
cmd.send_uart("No imu detected", True)
sensors.ina_init(i2c)
def go_home(self):
if self._planner_init is True:
self.status = "Start"
self._waypoints = [self._home]
self._next_waypoint = self._home
self._waypoints_num = 1
self._curr_waypoint_num = 1
self.resume_planner()
msg = {
"type": "gps",
"func": "waypoints",
"waypoints": self._waypoints
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
if debug_flag is True:
cmd.debug(msg, debug_flag)
else:
cmd.send_uart("Autonomous mode hasn't been started yet\n",
log_flag)
def pid(self):
# Updating the target variable
if self.target_flag is True:
# t_fn is planner.dist_bearing()
flag, (dist, bear) = self.t_fn()
self.target_flag = False
if flag is True:
self.target = bear
else:
return False
self.feedback = global_variables.fuse.heading
# Error term
self.error = self.target - self.feedback
# Always rotating in the shortest direction
if self.error > 180:
self.error = 360 - self.error
elif self.error < -180:
self.error = 360 + self.error
if (self.error > -20) and (self.error < 20):
self.error_sum += self.error
self.pid_out = self.Kp * self.error # Proportional
self.pid_out += self.Kd * self.prev_error # Derivative
self.pid_out += self.Ki * self.error_sum # Integral
self.prev_error = self.error
self.base(self.error)
# If error is negative (left turn needed to reduce error), so is pid_out. right speed increases, left speed decreases, vice versa
self.output = (round(
((self.base_speed + self.pid_out / 2) * self.LIMITER),
0), round(((self.base_speed - self.pid_out / 2) * self.LIMITER),
0))
if (abs(self.output[0]) < self.POWER_LIMIT) and (abs(self.output[1]) <
self.POWER_LIMIT):
motor.motor_abs(("l", self.output[0]), ("r", self.output[1]))
else:
cmd.send_uart("pid out of range\n", log_flag)
self.error_sum = 0
return True
def start_pid(self):
# Allows time for the first IMU reading to be completed
sleep_ms(12)
cmd.send_uart("PID thread started", True)
print("PID thread started")
self.enable = False
i = 0
while True:
if i % 5 == 0:
if self.enable:
self.pid()
if i % 3 == 0:
try:
acc = global_variables.imu.accel.xyz
gyr = global_variables.imu.gyro.xyz
mag = global_variables.imu.mag.xyz
except:
cmd.send_uart("MPU reading error", log_flag)
if i % 300 == 0:
msg = {
"type": "gps",
"func": "heading",
"head": str(global_variables.fuse.heading)
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
# print(global_variables.fuse.heading)
global_variables.fuse.update(acc, gyr, mag)
i += 1
def start_planner(self):
try:
if ((self._gps.pos_fix > 0) or
(self._gps.status == "A")) and self._gps.latitude is not None:
if self._planner_init is False:
if self.waypoint_init() is True:
self.status = "Start"
global_variables.pid_test.start_update()
cmd.send_uart("Autonomous mode started\n", log_flag)
self._planner_init = True
else:
cmd.send_uart("Couldn't initialise waypoints\n",
log_flag)
else:
self.status = "Start"
self.resume_planner()
else:
cmd.send_uart("GPS accuracy not good enough\n", log_flag)
except TypeError:
cmd.send_uart("No GPS signal\n", log_flag)
def waypoint_init(self):
try:
self._waypoints_num = len(self._waypoints) - 1
self._home = self._waypoints[0]
self._curr_waypoint_num = 0
self._next_waypoint = self._waypoints[0]
cmd.send_uart(
"{0:d} waypoints initialised\n".format(len(self._waypoints)),
log_flag)
msg = {
"type": "gps",
"func": "waypoints",
"waypoints": self._waypoints
}
msg = ujson.dumps(msg)
cmd.send_uart(msg, log_flag)
if debug_flag is True:
cmd.debug(msg, debug_flag)
return True
except (TypeError, IndexError):
cmd.send_uart("No waypoints", log_flag)
return False
def print_msg(self):
msg = self._uart.readline()
if msg is not None:
cmd.send_uart(msg, log_flag)
