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)