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