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 motor_rel(*commands):
flag = False
error_msg = 'Correct rel motor command'
for command in commands:
delta_pwr = command[1]
if command[0] == "l":
current_pwr = motor_power_left.pulse_width_percent()
current_dir = motor_direction_left.value()
if current_dir is 0:
current_pwr = -1*current_pwr
new_pwr = int(current_pwr + delta_pwr)
current_pwr_sign = True if current_pwr >= 0 else False
new_pwr_sign = True if new_pwr >= 0 else False
# Change the direction of rotation if change_dir is True
change_dir = current_pwr_sign ^ new_pwr_sign
new_pwr = abs(new_pwr)
if (100 >= new_pwr) and (new_pwr >= 0):
if change_dir is 1:
motor_direction_left.value(not current_dir)
motor_power_left.pulse_width_percent(new_pwr)
flag = False
elif new_pwr > 100:
new_pwr = 100
if change_dir is 1:
motor_direction_left.value(not current_dir)
motor_power_left.pulse_width_percent(new_pwr)
flag = True
error_msg = 'Rel command clipping at 100%, left'
elif command[0] == "r":
current_pwr = motor_power_right.pulse_width_percent()
current_dir = motor_direction_right.value()
if current_dir is 0:
current_pwr = -1*current_pwr
new_pwr = int(current_pwr + delta_pwr)
current_pwr_sign = True if current_pwr >= 0 else False
new_pwr_sign = True if new_pwr >= 0 else False
# Change the direction of rotation if change_dir is True
change_dir = current_pwr_sign ^ new_pwr_sign
new_pwr = abs(new_pwr)
if (100 >= new_pwr) and (new_pwr >= 0):
if change_dir is 1:
motor_direction_right.value(not current_dir)
motor_power_right.pulse_width_percent(new_pwr)
flag = False
elif new_pwr > 100:
new_pwr = 100
if change_dir is 1:
motor_direction_right.value(not current_dir)
motor_power_right.pulse_width_percent(new_pwr)
flag = True
error_msg = 'Rel command clipping at 100%, right'
if debug_flag is True:
cmd.debug("Current pwr: {0} {1:d}".format(str(command[0]), new_pwr), debug_flag)
return flag, error_msg
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 motor_abs(*commands):
for command in commands:
pwr = command[1]
if debug_flag is True:
cmd.debug("side: {0} pwr: {1}".format(str(command[0]), str(pwr)), debug_flag)
# Forward
if pwr >= 0:
direction = True
# Backward
elif pwr < 0:
direction = False
# pulse_width has to be positive
pwr = -1*pwr
if command[0] == "l":
motor_direction_left.value(direction)
motor_power_left.pulse_width_percent(pwr)
elif command[0] == "r":
motor_direction_right.value(direction)
motor_power_right.pulse_width_percent(pwr)
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 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 set_pid(self, P, I, D):
self.Kp = P
self.Ki = I
self.Kd = D
if debug_flag is True:
cmd.debug("PID set", debug_flag)