def readSonar():
LMeasure = []
RMeasure = []
for i in range(0, 3):
LMeasure.append(Sonar.measure(serialDevice, LeftSonarIndicator))
RMeasure.append(Sonar.measure(serialDevice, RightSonarIndicator))
LAvg = sum(LMeasure) / len(LMeasure)
RAvg = sum(RMeasure) / len(RMeasure)
return LAvg, RAvg
def sonar_detect():
serialPort = "/dev/ttyAMA0"
maxRange = 5000 # change for 5m vs 10m sensor
sleepTime = 1
minMM = 9999
maxMM = 0
t = 0
text_file = open("Sonar.txt", "w")
while t < 2:
mm = maxSonarTTY.measure(serialPort)
if mm >= maxRange:
#print("no target")
sleep(sleepTime)
continue
if mm < minMM:
minMM = mm
if mm > maxMM:
maxMM = mm
#print("distance:", mm, " min:", minMM, "max:", maxMM)
text_file.write(str(mm) + " ")
sleep(1)
t = t + 1
text_file.close()
def us_initialization(self, serialPort):
print("Running Ultrasonic Sensor ", 10, " times.")
height_corr = 0
for x in range(0, 9):
height_corr += maxSonarTTY.measure(serialPort)
height_corr /= 10
print("Height At Initilization: ", height_corr)
return height_corr
def initialization(self, serialPort):
print("Running Ultrasonic Sensor ",50, " times.")
heightCorr =0
for x in range(0, 49):
heightCorr += maxSonarTTY.measure(serialPort)
heightCorr/=50
print(heightCorr)
print("Height Initialized")
time.sleep(0.5)
return heightCorr
def dist_sendor(self):
#data = Range()
while not rospy.is_shutdown():
self.mm = maxSonarTTY.measure(self.serialPort0)
if self.mm >= self.maxRange:
print("no target")
time.sleep(self.sleepTime)
continue
# ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
self._range.range = self.mm * 0.001
self._range.header.frame_id = "/sonarD_link"
self.distance_publisher_down.publish(self._range)
time.sleep(0.01)
def __init__(self, name, highcut, fs):
self.highcut = highcut
self.fs = fs
self.order = 1
self.B, self.A = self.butter_lowpass(self.highcut, self.fs, self.order)
# ini data
nA = len(self.A)
nB = len(self.B)
x0 = maxSonarTTY.measure(serialPort)
x0 = float(x0)
self.X = np.ones([1, nB]) * x0
self.Y = np.ones([1, nA]) * x0
# ROS functions
self.talker()
def updateState(self, flightmode, adc, gain, flag, serialPort):
self.FlightMode = flightmode
self.pressure[0] = voltToPressure(
valToVolt(adc.read_adc(0, gain), gain))
self.pressure[1] = voltToPressure(
valToVolt(adc.read_adc(1, gain), gain))
self.pressure[2] = voltToPressure(
valToVolt(adc.read_adc(2, gain), gain))
self.currentTime = 0
self.terminator = flag
self.velocity[0] = 0
self.velocity[1] = 0
self.velocity[2] = 0
self.orientation[0] = 0
self.orientation[1] = 0
self.orientation[2] = maxSonarTTY.measure(serialPort)
def update_state(self, flightmode, adc, gain, serialPort, TheVehicle,
datafile):
while (self.terminator == 0):
self.theTime = time.time()
dt = time.time() - self.theTime_prev
self.flight_mode = flightmode
self.pressure[0] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(0, gain), gain))
self.pressure[1] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(1, gain), gain))
self.pressure[2] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(2, gain), gain))
if (TEST_MODE == 0):
self.position[0] = 0.0
self.position[1] = 0.0
self.position[2] = 0.0254 * (maxSonarTTY.measure(serialPort) -
self.height_corr)
self.velocity[0] = 0.0
self.velocity[1] = 0.0
self.velocity[2] = (
(self.position[2] - self.position_prev[2]) / dt)
elif (TEST_MODE == 1):
self.mass_water_model -= m_dot_max * dt * self.solenoid_state
mass_tot_new = self.mass_tot - m_dot_max * dt * self.solenoid_state
if (self.mass_water_model <= 0):
print("Out of Water")
self.terminator = 1
mass_tot_new = mass_dry
dv = GRAVITY * dt + (ue * log(self.mass_tot / mass_tot_new))
self.mass_tot = mass_tot_new
self.velocity_model[2] += dv
self.position_model[2] = self.velocity_model[2] * dt
if (self.position_model[2] <= 0):
self.velocity_model[
2] = 0 #review algorithm order of this assignment
self.position_model[2] = 0
self.velocity[2] = self.velocity_model[2]
self.position[2] = self.position_model[2]
self.position_prev = self.position
self.theTime_prev = self.theTime
print("velocity: ", self.velocity[2], "height ", self.position[2])
def talker(self):
rospy.init_node('sonar', anonymous=True)
pub = rospy.Publisher('sonar_dist', String, queue_size=1)
rate = rospy.Rate(self.fs) # 1hz
while not rospy.is_shutdown():
try:
mm = maxSonarTTY.measure(serialPort)
# rospy.loginfo(mm)
filtered_mm = self.filter_data(mm)
pub.publish(str(filtered_mm))
# rospy.loginfo(filtered_mm)
except:
sonar_error = 'No sonar Data'
rospy.loginfo(sonar_error)
rate.sleep()
def updateState(self, PreviousState, flightmode, adc, gain, serialPort):
while (self.terminator==0):
self.theTime = time.time()
dt = PreviousState.theTime = self.theTime
self.FlightMode = flightmode
self.pressure[0] = utils.voltToPressure(utils.valToVolt(adc.read_adc(0, gain), gain))
self.pressure[1] = utils.voltToPressure(utils.valToVolt(adc.read_adc(1, gain), gain))
self.pressure[2] = utils.voltToPressure(utils.valToVolt(adc.read_adc(2, gain), gain))
self.position[0] = 0.0
self.position[1] = 0.0
self.position[2] = maxSonarTTY.measure(serialPort) - self.heightCorr
self.velocity[0] = 0.0
self.velocity[1] = 0.0
self.velocity[2] = ((self.position[2] - PreviousState.position[2])/dt)
print("Don't worry, I am updating too, honey! ", self.terminator)
def updateState(self, PreviousState, flightmode, adc, gain, flag,
serialPort):
self.theTime = time.time()
dt = PreviousState.theTime - self.theTime
self.FlightMode = flightmode
self.pressure[0] = utils.voltToPressure(
utils.valToVolt(adc.read_adc(0, gain), gain))
self.pressure[1] = utils.voltToPressure(
utils.valToVolt(adc.read_adc(1, gain), gain))
self.pressure[2] = utils.voltToPressure(
utils.valToVolt(adc.read_adc(2, gain), gain))
self.terminator = flag
self.orientation[0] = 0.0
self.orientation[1] = 0.0
self.orientation[2] = maxSonarTTY.measure(serialPort)
self.velocity[0] = 0.0
self.velocity[1] = 0.0
self.velocity[2] = (
(self.orientation[2] - PreviousState.orientation[2]) / dt)
def dist_sendor(self):
#data = Range()
while not rospy.is_shutdown():
#rospy.sleep(1.0)
for idx in self.frames:
if (idx == '/sonarD_link'):
self.mm = maxSonarTTY.measure(self.serialPort0)
if self.mm >= self.maxRange:
print("no target")
time.sleep(self.sleepTime)
continue
# ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
self._range.range = self.mm * 0.001
self._range.header.frame_id = idx
self.distance_publisher_down.publish(self._range)
#time.sleep(0.2)
Serial(self.serialPort0, 9600, 8, 'N', 1).flushInput()
Serial(self.serialPort0, 9600, 8, 'N', 1).flushOutput()
Serial(self.serialPort0, 9600, 8, 'N', 1).close()
from time import sleep
import maxSonarTTY
serialPort = "/dev/ttyS0"
maxRange = 5000 # change for 5m vs 10m sensor
sleepTime = 5
minMM = 9999
maxMM = 0
while True:
mm = maxSonarTTY.measure(serialPort)
if mm >= maxRange:
print("no target")
sleep(sleepTime)
continue
if mm < minMM:
minMM = mm
if mm > maxMM:
maxMM = mm
print("distance:", mm, " min:", minMM, "max:", maxMM)
sleep(sleepTime)
def update_state(self, adc, gain, serialPort, TheVehicle):
while (self.terminator[0] == 0 or self.terminator[1] == 0):
self.theTime = time.time()
dt = time.time() - self.theTime_prev
self.flight_mode = TheVehicle.flight_mode
self.pressure[0] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(0, gain), gain))
self.pressure[1] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(1, gain), gain))
self.pressure[2] = utils.volt_to_pressure(
utils.val_to_volt(adc.read_adc(2, gain), gain))
self.mass_water_model -= m_dot_max * dt * self.solenoid_state
mass_tot_new = self.mass_tot - m_dot_max * dt * self.solenoid_state
if (self.mass_water_model <= 0):
print("US: Out of Water")
self.terminator[0] = 1
mass_tot_new = mass_dry
if self.time_no_water == 0: #if this is the first time through this loop, mark the time
self.time_no_water = time.time()
if (TEST_MODE != 1): #==0
self.position[0] = 0.0
self.position[1] = 0.0
self.position[2] = 0.0254 * (maxSonarTTY.measure(serialPort) -
self.height_corr)
self.velocity[0] = 0.0
self.velocity[1] = 0.0
self.velocity[2] = (
(self.position[2] - self.position_prev[2]) / dt)
elif (TEST_MODE == 1):
dv = GRAVITY * dt + (ue * log(self.mass_tot / mass_tot_new))
self.velocity_model[2] += dv
self.position_model[2] += self.velocity_model[2] * dt
if (self.position_model[2] <= 0):
self.velocity_model[
2] = 0 #review algorithm order of this assignment
self.position_model[2] = 0
if self.time_no_altitude == 0 and self.mass_water_model <= 0: #if first time through loop, mark the time
self.time_no_altitude = time.time()
print("Time H20 runs out: ",
(self.time_no_water - self.time_start))
print("Time hits ground: ",
(self.time_no_altitude - self.time_start))
TheVehicle.abort(self)
self.velocity[2] = self.velocity_model[2]
self.position[2] = self.position_model[2]
self.mass_tot = mass_tot_new
self.position_prev = self.position
self.theTime_prev = self.theTime
print("US: velocity: ", self.velocity[2], "height ",
self.position[2], "Water Remaining: ", self.mass_water_model)
self.datafile.write("," + str(self.theTime) + "," + str(dt) + "," +
str(self.position[0]) + "," +
str(self.position[1]) + "," +
str(self.position[2]) + "," +
str(self.velocity[0]) + "," +
str(self.velocity[1]) + "," +
str(self.velocity[2]) + "," +
str(self.pressure[0]) + "," +
str(self.pressure[1]) + "," +
str(self.pressure[2]) + "\n")
SONAR_THRESH = 1500
# Motor Constants
MOTOR_NORMAL_SP = 360
MOTOR_MAIN_TURN_SP = 240
MOTOR_OFF_TURN_SP = 120
TURN_TIME = 1
MOTOR_FWD = 1
MOTOR_BKWD = 0
def readSonar():
LMeasure = []
RMeasure = []
for i in range(0,3):
LMeasure.append(Sonar.measure(serialDevice, LeftSonarIndicator))
RMeasure.append(Sonar.measure(serialDevice, RightSonarIndicator))
LAvg = sum(LMeasure) / len(LMeasure)
RAvg = sum(RMeasure) / len(RMeasure)
return LAvg, RAvg
# Algorithm State
class State(Enum):
FORWARD = 0
AVOIDANCE = 1
STALL = 2
DEPLOY_SONAR = 3
REVERSE = 4
