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.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.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
