Example #1
0
    def __init__(self):
        self.state = State(0)
        self.params = Params()
        self.status = Status(self.params)
        self.sleep_time = 1
        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..")
        self.iSensor = ina226(INA226_ADDRESS, 1)
        self.iSensor.configure(
            avg=ina226_averages_t["INA226_AVERAGES_4"],
        )
        self.iSensor.calibrate(rShuntValue=0.002, iMaxExcepted=1)

        time.sleep(1)

        print("Configuration Done")

        current = self.iSensor.readShuntCurrent()

        print("Current Value is " + str(current) + "A")

        print("Mode is " + str(hex(self.iSensor.getMode())))
    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")
Example #3
0
 def __init__(self):
     self.state = State(0)
     self.params = Params()
     self.status = Status(self.params)
     self.sleep_time = 1
     self.pwm_read = PwmRead(self.params.pin_mode_in, self.params.pin_servo_in, self.params.pin_thruster_in)
     self.pwm_out = PwmOut(self.params.pin_servo_out, self.params.pin_thruster_out)
     self.pid = PositionalPID()
     self.logger = Logger()
     self.logger.open()
Example #4
0
class Driver:
    def __init__(self):
        self.state = State(0)
        self.params = Params()
        self.status = Status(self.params)
        self.sleep_time = 1
        self.pwm_read = PwmRead(self.params.pin_mode_in,
                                self.params.pin_servo_in,
                                self.params.pin_thruster_in)
        self.pwm_out = PwmOut(self.params.pin_servo_out,
                              self.params.pin_thruster_out)
        self.pid = PositionalPID()
        self.logger = Logger()
        self.logger.open()

    def load(self, filename):
        print('loading', filename)
        f = open(filename, "r")

        line = f.readline()
        line = f.readline()
        self.state.time_limit = int(line.split()[1])  # Time Limit
        line = f.readline()
        self.sleep_time = float(line.split()[1])  # Sleep time

        line = f.readline()
        line = f.readline()
        line = f.readline()
        p = float(line.split()[1])  # P
        line = f.readline()
        i = float(line.split()[1])  # I
        line = f.readline()
        d = float(line.split()[1])  # D
        self.pid.setPID(p, i, d)

        line = f.readline()
        line = f.readline()
        line = f.readline()
        num = int(line.split()[1])  # Number of waypoints
        line = f.readline()
        for i in range(num):
            line = f.readline()
            self.status.waypoint.addPoint(float(line.split()[0]),
                                          float(line.split()[1]))
        f.close()
        return

    def doOperation(self):
        while self.state.inTimeLimit():
            self.readPWM()
            self.readGps()

            mode = self.getMode()
            if mode == 'RC':
                self.remoteControl()
            elif mode == 'AN':
                self.autoNavigation()

            self.outPWM()
            self.printLog()
            time.sleep(self.sleep_time)
        return

    def getMode(self):
        return self.status.mode

    def updateMode(self):
        mode_duty_ratio = self.pwm_read.pulse_width[0]
        if mode_duty_ratio < 1500:
            self.status.mode = 'RC'
        elif mode_duty_ratio >= 1500:
            self.status.mode = 'AN'
        return

    def readGps(self):
        self.status.readGps()
        self.updateMode()
        #if self.status.isGpsError():
        #self.status.mode = 'RC'
        return

    def updateStatus(self):
        status = self.status
        status.calcTargetDirection()
        status.calcTargetDistance()
        status.updateTarget()
        return

    def readPWM(self):
        self.pwm_read.measurePulseWidth()
        self.pwm_out.servo_pulsewidth = self.pwm_read.pulse_width[1]
        self.pwm_out.thruster_pulsewidth = self.pwm_read.pulse_width[2]
        return

    def outPWM(self):
        self.pwm_out.updatePulsewidth()
        return

    def autoNavigation(self):
        self.updateStatus()
        boat_direction = self.status.boat_direction
        target_direction = self.status.target_direction
        servo_pulsewidth = self.pid.getStepSignal(target_direction,
                                                  boat_direction)
        self.pwm_out.servo_pulsewidth = servo_pulsewidth
        self.pwm_out.thruster_pulsewidth = 1880
        return

    def remoteControl(self):
        # Do nothing
        return

    def printLog(self):
        timestamp_string = self.status.timestamp_string
        mode = self.getMode()
        latitude = self.status.latitude
        longitude = self.status.longitude
        speed = self.status.speed
        direction = self.status.boat_direction
        servo_pw = self.pwm_out.servo_pulsewidth
        thruster_pw = self.pwm_out.thruster_pulsewidth
        t_direction = self.status.target_direction
        t_distance = self.status.target_distance
        target = self.status.waypoint.getPoint()
        t_latitude = target[0]
        t_longitude = target[1]
        print(timestamp_string)
        print(
            '[%s MODE] LAT=%.7f, LON=%.7f, SPEED=%.2f [km/h], DIRECTION=%lf' %
            (mode, latitude, longitude, speed, direction))
        print('DUTY (SERVO, THRUSTER):       (%lf, %lf) [us]' %
              (servo_pw, thruster_pw))
        print('TARGET (LATITUDE, LONGITUDE): (%lf, %lf)' %
              (t_latitude, t_longitude))
        print('TARGET (DIRECTION, DISTANCE): (%lf, %lf [m])' %
              (t_direction, t_distance))
        print('')
        log_list = [
            timestamp_string, mode, latitude, longitude, direction, t_latitude,
            t_longitude, servo_pw, thruster_pw, t_direction, t_distance
        ]
        self.logger.write(log_list)
        return

    def finalize(self):
        self.logger.close()
        self.pwm_out.finalize()
        return
Example #5
0
class Driver:
    def __init__(self):
        self.state = State(0)
        self.params = Params()
        self.status = Status(self.params)
        self.sleep_time = 1
        self.pwm_read = PwmRead(self.params.pin_mode_in,
                                self.params.pin_servo_in,
                                self.params.pin_thruster_in)
        self.pwm_out = PwmOut(self.params.pin_servo_out,
                              self.params.pin_thruster_out)
        self.pid = PositionalPID()
        self.logger = Logger()
        self.logger.open()

    def load(self, filename):
        print('loading', filename)
        f = open(filename, "r")

        line = f.readline()
        line = f.readline()
        self.state.time_limit = int(line.split()[1])  # Time Limit
        line = f.readline()
        self.sleep_time = float(line.split()[1])  # Sleep time

        line = f.readline()
        line = f.readline()
        line = f.readline()
        self.pwm_read.num_cycles = int(line.split()[1])  # NUM_CYCLE

        line = f.readline()
        line = f.readline()
        line = f.readline()
        self.pwm_out.coefficient = float(line.split()[1])  # Coefficient

        line = f.readline()
        line = f.readline()
        line = f.readline()
        self.pid.angular_range = int(line.split()[1])  # angular_range
        line = f.readline()
        p = float(line.split()[1])  # P
        line = f.readline()
        i = float(line.split()[1])  # I
        line = f.readline()
        d = float(line.split()[1])  # D
        self.pid.setPID(p, i, d)

        line = f.readline()
        line = f.readline()
        line = f.readline()
        self.status.waypoint_radius = float(
            line.split()[1])  # range of target point
        line = f.readline()
        num = int(line.split()[1])  # Number of waypoints
        line = f.readline()
        for i in range(num):
            line = f.readline()
            self.status.waypoint.addPoint(float(line.split()[0]),
                                          float(line.split()[1]))
        f.close()
        return

    def doOperation(self):
        while self.state.inTimeLimit():
            self.readPWM()
            self.readGps()

            mode = self.getMode()
            if mode == 'RC':
                self.remoteControl()
            elif mode == 'AN':
                self.autoNavigation()

            self.outPWM()
            self.printLog()
            time.sleep(self.sleep_time)
        return

    def getMode(self):
        return self.status.mode

    def updateMode(self):
        mode_duty_ratio = self.pwm_read.pulse_width[0]
        if mode_duty_ratio < 1500:
            self.status.mode = 'RC'
        elif mode_duty_ratio >= 1500:
            self.status.mode = 'AN'
        return

    def readGps(self):
        self.status.readGps()
        self.updateMode()
        #if self.status.isGpsError():
        #self.status.mode = 'RC'
        return

    def updateStatus(self):
        status = self.status
        status.calcTargetDirection()
        status.calcTargetDistance()
        status.updateTarget()
        return

    # Read pwm pulsewidth
    # Set the readout signals as the output signals
    def readPWM(self):
        self.pwm_read.measurePulseWidth()
        self.pwm_out.servo_pulsewidth = self.pwm_read.pulse_width[1]
        self.pwm_out.thruster_pulsewidth = self.pwm_read.pulse_width[2]
        return

    def outPWM(self):
        self.pwm_out.updatePulsewidth()
        return

    def autoNavigation(self):
        self.updateStatus()
        if self.status.mode != 'AN_END':
            boat_direction = self.status.boat_direction
            target_direction = self.status.target_direction
            servo_pulsewidth = self.pid.getStepSignal(target_direction,
                                                      boat_direction)
            self.pwm_out.servo_pulsewidth = servo_pulsewidth
            self.pwm_out.thruster_pulsewidth = 1880  #if thruster do not run, change this to a relatively small value(max=1900)
            return
        else:
            # If the boat has passed the last waypoint,
            # the navigation system get RC mode.
            return

    def remoteControl(self):
        # Do nothing
        return

    def printLog(self):
        timestamp_string = self.status.timestamp_string
        mode = self.getMode()
        latitude = self.status.latitude
        longitude = self.status.longitude
        speed = self.status.speed
        direction = self.status.boat_direction
        servo_pw = self.pwm_out.servo_pulsewidth
        thruster_pw = self.pwm_out.thruster_pulsewidth
        t_index = self.status.waypoint.getIndex()
        t_direction = self.status.target_direction
        t_distance = self.status.target_distance
        target = self.status.waypoint.getPoint()
        t_latitude = target[0]
        t_longitude = target[1]
        err = self.pid.ErrBack

        # To print logdata
        print(timestamp_string)
        print(
            '[%s MODE] LAT=%.7f, LON=%.7f, SPEED=%.2f [km/h], DIRECTION=%lf' %
            (mode, latitude, longitude, speed, direction))
        print('DUTY (SERVO, THRUSTER):       (%6.1f, %6.1f) [us]' %
              (servo_pw, thruster_pw))
        print('TARGET No.%2d' % (t_index))
        print('TARGET (LATITUDE, LONGITUDE): (%.7f, %.7f)' %
              (t_latitude, t_longitude))
        print('TARGET (DIRECTION, DISTANCE): (%5.2f, %5.2f [m])' %
              (t_direction, t_distance))
        print('')

        # To write logdata (csv file)
        log_list = [
            timestamp_string, mode, latitude, longitude, direction, speed,
            t_index, t_latitude, t_longitude, t_direction, err
        ]
        self.logger.write(log_list)
        return

    def finalize(self):
        self.logger.close()
        self.pwm_out.finalize()
        return
Example #6
0
class Driver:
    def __init__(self):
        self.state = State(0)
        self.params = Params()
        self.status = Status(self.params)
        self.sleep_time = 1
        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..")
        self.iSensor = ina226(INA226_ADDRESS, 1)
        self.iSensor.configure(
            avg=ina226_averages_t["INA226_AVERAGES_4"],
        )
        self.iSensor.calibrate(rShuntValue=0.002, iMaxExcepted=1)

        time.sleep(1)

        print("Configuration Done")

        current = self.iSensor.readShuntCurrent()

        print("Current Value is " + str(current) + "A")

        print("Mode is " + str(hex(self.iSensor.getMode())))

    def load(self, filename):
        print("loading", filename)
        f = open(filename, "r")

        line = f.readline()
        line = f.readline()
        self.state.time_limit = int(line.split()[1])  # Time Limit
        line = f.readline()
        self.sleep_time = float(line.split()[1])  # Sleep time

        line = f.readline()
        line = f.readline()
        line = f.readline()
        p = float(line.split()[1])  # P
        line = f.readline()
        i = float(line.split()[1])  # I
        line = f.readline()
        d = float(line.split()[1])  # D
        self.pid.setPID(p, i, d)

        line = f.readline()
        line = f.readline()
        line = f.readline()
        num = int(line.split()[1])  # Number of waypoints
        line = f.readline()
        for i in range(num):
            line = f.readline()
            self.status.waypoint.addPoint(
                float(line.split()[0]), float(line.split()[1])
            )
        f.close()
        return

    def doOperation(self):
        while self.state.inTimeLimit():
            self.readPWM()
            self.readGps()

            # for test
            self.pwm_read.printPulseWidth()
            # ina226
            print(
                "Current: "
                + str(round(self.iSensor.readShuntCurrent(), 3))
                + "A"
                + ", Voltage: "
                + str(round(self.iSensor.readBusVoltage(), 3))
                + "V"
                + ", Power:"
                + str(round(self.iSensor.readBusPower(), 3))
                + "W"
            )

            mode = self.getMode()
            if mode == "RC":
                self.remoteControl()
            elif mode == "AN":
                self.autoNavigation()
            elif mode == "OR":
                self.outOfRangeOperation()

            self.outPWM()
            self.printLog()
            time.sleep(self.sleep_time)
        return

    def getMode(self):
        return self.status.mode

    def updateMode(self):
        mode_duty_ratio = self.pwm_read.pulse_width[0]
        or_pulse = self.pwm_read.pulse_width[3]
        # OR mode
        if or_pulse < 1300 or (1500 <= mode_duty_ratio and self.or_experienced):
            if not self.or_experienced:
                self.status.updateWayPoint()
            self.status.mode = "OR"
            self.or_experienced = True
        # RC mode
        elif 0 < mode_duty_ratio < 1500:
            self.status.mode = "RC"
        # AN mode
        elif 1500 <= mode_duty_ratio and not self.or_experienced:
            self.status.mode = "AN"
        else:
            print("Error: mode updating failed", file=sys.stderr)
        return

    def readGps(self):
        self.status.readGps()
        self.updateMode()
        # if self.status.isGpsError():
        # self.status.mode = 'RC'
        return

    def updateStatus(self):
        status = self.status
        status.calcTargetDirection()
        status.calcTargetDistance()
        status.updateTarget()
        return

    # Read pwm pulsewidth
    # Set the readout signals as the output signals
    def readPWM(self):
        self.pwm_read.measurePulseWidth()
        self.pwm_out.servo_pulsewidth = self.pwm_read.pulse_width[1]
        self.pwm_out.thruster_pulsewidth = self.pwm_read.pulse_width[2]
        return

    def outPWM(self):
        self.pwm_out.updatePulsewidth()
        return

    def autoNavigation(self):
        self.updateStatus()
        boat_direction = self.status.boat_direction
        target_direction = self.status.target_direction
        servo_pulsewidth = self.pid.getStepSignal(target_direction, boat_direction)
        self.pwm_out.servo_pulsewidth = servo_pulsewidth
        self.pwm_out.thruster_pulsewidth = 1700
        return

    def remoteControl(self):
        # Do nothing
        return

    def outOfRangeOperation(self):
        # Be stationary
        # self.pwm_out.finalize()
        # update waypoint where the boat was
        self.autoNavigation()
        return

    def printLog(self):
        timestamp_string = self.status.timestamp_string
        mode = self.getMode()
        latitude = self.status.latitude
        longitude = self.status.longitude
        speed = self.status.speed
        direction = self.status.boat_direction
        servo_pw = self.pwm_out.servo_pulsewidth
        thruster_pw = self.pwm_out.thruster_pulsewidth
        t_direction = self.status.target_direction
        t_distance = self.status.target_distance
        target = self.status.waypoint.getPoint()
        t_latitude = target[0]
        t_longitude = target[1]
        err = self.pid.ErrBack
        current = str(round(self.iSensor.readShuntCurrent(), 3))
        voltage = str(round(self.iSensor.readBusVoltage(), 3))
        power = str(round(self.iSensor.readBusPower(), 3))

        # To print logdata
        print(timestamp_string)
        print(
            "[%s MODE] LAT=%.7f, LON=%.7f, SPEED=%.2f [km/h], DIRECTION=%lf"
            % (mode, latitude, longitude, speed, direction)
        )
        print(
            "DUTY (SERVO, THRUSTER):       (%6.1f, %6.1f) [us]"
            % (servo_pw, thruster_pw)
        )
        print("TARGET (LATITUDE, LONGITUDE): (%.7f, %.7f)" % (t_latitude, t_longitude))
        print(
            "TARGET (DIRECTION, DISTANCE): (%5.2f, %5.2f [m])"
            % (t_direction, t_distance)
        )
        print("")

        # To write logdata (csv file)
        log_list = [
            timestamp_string,
            mode,
            latitude,
            longitude,
            direction,
            speed,
            t_latitude,
            t_longitude,
            servo_pw,
            thruster_pw,
            t_direction,
            t_distance,
            err,
            current,
            voltage,
            power,
        ]
        self.logger.write(log_list)
        return

    def finalize(self):
        self.logger.close()
        self.pwm_out.finalize()
        return
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