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")
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()
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
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
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