class Driver: def __init__(self): self._time_manager = TimeManager() self._params = Params() self._status = Status(self._params) self.log_time = time.time() self._pwm_read = PwmRead( self._params.pin_mode_in, self._params.pin_servo_in, self._params.pin_thruster_in, self._params.pin_or, ) self._pwm_out = PwmOut(self._params.pin_servo_out, self._params.pin_thruster_out) self._pid = PositionalPID() self._logger = Logger() self._logger.open() # Whether experienced OR mode or not self._or_experienced = False # setup for ina226 print("Configuring INA226..") try: self.i_sensor = ina226(INA226_ADDRESS, 1) self.i_sensor.configure( avg=ina226_averages_t["INA226_AVERAGES_4"], ) self.i_sensor.calibrate(rShuntValue=0.002, iMaxExcepted=1) self.i_sensor.log() print("Mode is " + str(hex(self.i_sensor.getMode()))) except: print("Error when configuring INA226") time.sleep(1) print("Configuration Done") def check_mode_change(self): print( "Please set to AN mode and then switch to RC mode to start appropriately." ) self._pwm_read.measure_pulse_width() self._update_mode() if self._status.mode == "AN": print("Next procedure: Set to RC mode to start.") while self._status.mode == "AN": self._pwm_read.measure_pulse_width() self._update_mode() time.sleep(0.1) elif self._status.mode == "RC": print( "Next procedure: set to AN mode and then switch to RC mode to start." ) while self._status.mode == "RC": self._pwm_read.measure_pulse_width() self._update_mode() time.sleep(0.1) print("Next procedure: Set to RC mode to start.") while self._status.mode == "AN": self._pwm_read.measure_pulse_width() self._update_mode() time.sleep(0.1) print("Procedure confirmed.") def load_params(self, filename): print("loading", filename) with open(filename, "r") as f: params = yaml.safe_load(f) time_limit = params["time_limit"] sleep_time = params["sleep_time"] P = params["P"] I = params["I"] D = params["D"] self._time_manager.set_time_limit(time_limit) # Time Limit self._sleep_time = float(sleep_time) # Sleep time self._pid.set_pid(P, I, D) for wp in params["waypoints"]: name = wp["name"] lat = wp["lat"] lon = wp["lon"] print(name, lat, lon) self._status.waypoint.add_point(lat, lon) return def do_operation(self): while self._time_manager.in_time_limit(): # update pwm # Read pwm pulse width self._pwm_read.measure_pulse_width() # Set the readout signals as the output signals # self._pwm_out.mode_pulse_width = self._pwm_read.pins[ # self._pwm_read.pin_mode # ]["pulse_width"] self._pwm_out.servo_pulse_width = self._pwm_read.pins[ self._pwm_read.pin_servo]["pulse_width"] self._pwm_out.thruster_pulse_width = self._pwm_read.pins[ self._pwm_read.pin_thruster]["pulse_width"] # read gps self._status.read_gps() self._update_mode() mode = self._status.mode if mode == "RC": pass elif mode == "AN": self._auto_navigation() elif mode == "OR": self._out_of_range_operation() # update output self._pwm_out.update_pulse_width() if time.time() - self.log_time > 1: self.log_time = time.time() # for test self._pwm_read.print_pulse_width() # ina226 if hasattr(self, "i_sensor"): self.i_sensor.log() self._print_log() time.sleep(self._sleep_time) return def _update_mode(self): mode_duty_ratio = self._pwm_read.pins[ self._pwm_read.pin_mode]["pulse_width"] # RC mode if 0 < mode_duty_ratio < 1500: self._status.mode = "RC" # AN mode elif 1500 <= mode_duty_ratio: self._status.mode = "AN" else: print("Error: mode updating failed", file=sys.stderr) return def _auto_navigation(self): # update status status = self._status status.calc_target_bearing() status.calc_target_distance() status.update_target() boat_heading = math.degrees(self._status.boat_heading) target_bearing = math.degrees(self._status.target_bearing) target_bearing_relative = math.degrees( self._status.target_bearing_relative) target_distance = self._status.target_distance servo_pulse_width = self._pid.get_step_signal(target_bearing_relative, target_distance) self._pwm_out.servo_pulse_width = servo_pulse_width self._pwm_out.thruster_pulse_width = 1700 return def _out_of_range_operation(self): # Be stationary # self.pwm_out.end() # update waypoint where the boat was self._auto_navigation() return def _print_log(self): timestamp = self._status.timestamp_string mode = self._status.mode latitude = self._status.latitude longitude = self._status.longitude speed = self._status.speed heading = math.degrees(self._status.boat_heading) servo_pw = self._pwm_out.servo_pulse_width thruster_pw = self._pwm_out.thruster_pulse_width t_bearing = math.degrees(self._status.target_bearing) t_bearing_rel = math.degrees(self._status.target_bearing_relative) t_distance = self._status.target_distance target = self._status.waypoint.get_point() t_latitude = target[0] t_longitude = target[1] t_idx = self._status.waypoint._index err = self._pid.err_back if hasattr(self, "i_sensor"): current = str(round(self.i_sensor.readShuntCurrent(), 3)) voltage = str(round(self.i_sensor.readBusVoltage(), 3)) power = str(round(self.i_sensor.readBusPower(), 3)) else: current = 0 voltage = 0 power = 0 # To print logdata print(timestamp) print("[%s MODE] LAT=%.7f, LON=%.7f, SPEED=%.2f [km/h], HEADING=%lf" % (mode, latitude, longitude, speed, heading)) print("DUTY (SERVO, THRUSTER): (%6.1f, %6.1f) [us]" % (servo_pw, thruster_pw)) print(f"TARGET INDEX: {t_idx}") print("TARGET (LATITUDE, LONGITUDE): (%.7f, %.7f)" % (t_latitude, t_longitude)) print("TARGET (REL_BEARING, DISTANCE): (%5.2f, %5.2f [m])" % (t_bearing_rel, t_distance)) print("") # To write logdata (csv file) log_list = [ timestamp, mode, latitude, longitude, heading, speed, t_latitude, t_longitude, servo_pw, thruster_pw, t_bearing, t_distance, err, current, voltage, power, ] self._logger.write(log_list) return def end(self): self._logger.close() self._pwm_read.end() self._pwm_out.end() return