def __init__(self): Robot.__init__(self) self._left_touch = Sensor("PORT_1", "touch") self._right_touch = Sensor("PORT_2", "touch") self._sonar = Sensor("PORT_4", "sonar") self.error = 0 BrickPiSetupSensors() #Send the properties of sensors to BrickPi
def delete_sensor_to_monitor(): sensor_uuid = request.args.get("uuid") res = Sensor.api_delete_sensor_to_monitor({'uuid': sensor_uuid}) if res: Response(json.dumps(res[0], indent=2, sort_keys=True), mimetype='application/json'), res[1] return redirect(url_for('uuid_management', uuid=sensor_uuid))
def delete_registered_sensor(): uuid_sensor = request.args.get('uuid') res = Sensor.delete_registered_sensor({'uuid': uuid_sensor}) if res[1] == 200: return redirect(url_for('registered_sensor')) else: return jsonify(res[0])
def approve_sensor(): uuid_sensor = request.args.get('uuid') res = Sensor.approve_sensor({'uuid': uuid_sensor}) if res[1] == 200: return redirect(url_for('pending_sensors')) else: return jsonify(res[0])
def var_init(self, amount_of_sensors, amount_of_targets, range_of_sensor, capacity, map_high, map_width): self.amount_of_sensors = amount_of_sensors self.amount_of_targets = amount_of_targets self.range_of_sensor = range_of_sensor self.capacity = capacity self.map_high = map_high self.map_width = map_width self.sensor_list = [] self.target_list = [] for x in range(0, amount_of_sensors): self.sensor_list.append( Sensor.Sensor( capacity, range_of_sensor, Point.Point( randrange(range_of_sensor, map_width - range_of_sensor), randrange(range_of_sensor, map_high - range_of_sensor)))) for x in range(0, amount_of_targets): self.target_list.append( Target.Target( Point.Point( randrange(map_width - 2 * 0.1 * map_width) + 0.1 * map_width, randrange(map_high - 2 * 0.1 * map_high) + 0.1 * map_width)))
def initializeTrainTables(Trains, bigTable): for trainID in bigTable.subwayIds: subwayNum = trainID Trains[subwayNum] = Train.Train(subwayNum) #adding 12 dummy sensor nodes temporary_temp_int = int(time.strftime("%M")) for i in range(1, 13): curTrain_DummySensorNode = SensorNode.SensorNode( i, 'TempHum', False) curTrain_DummySensorNode.sensors['temp'] = Sensor.Sensor( 'temp', 'farenheit', temporary_temp_int) #13) curTrain_DummySensorNode.sensors['hum'] = Sensor.Sensor( 'hum', '%', temporary_temp_int) #13) Trains[subwayNum].sensorNodes[i] = curTrain_DummySensorNode
def setCurrentStatus( self, isalive, inp_sensors): #isalive:bool, inp_sensors: list of Sensor self.is_alive = isalive #if isalive == True: #print 'len(inp_sensors) = ', len(inp_sensors) #print 'inp_sensors:', inp_sensors print 'self.sensors:', self.sensors for s_name in inp_sensors: print "s_name= ", s_name print "sensorname, sensor :", inp_sensors[ s_name].sensorname, inp_sensors[ s_name].measurement, inp_sensors[ s_name].value #, inp_sensors[s_id] if s_name in self.sensors: print "using existing Sensor.Sensor value =", inp_sensors[ s_name].value self.sensors[s_name].sensorname = inp_sensors[ s_name].sensorname self.sensors[s_name].measurement = inp_sensors[ s_name].measurement self.sensors[s_name].value = int(inp_sensors[s_name].value) else: print "creation of New Sensor.Sensor value = ", inp_sensors[ s_name].value self.sensors[s_name] = Sensor.Sensor( inp_sensors[s_name].sensorname, inp_sensors[s_name].measurement, int(inp_sensors[s_name].value))
def second_funck_for_90dl(dist_from_f_mid): try: print(dist_from_f_mid) dist_from_right = [] dist_from_right.append(s.r_sensor()) print(dist_from_right) sum_dist = [] sum_dist.append(abs(dist_from_right[len(dist_from_right) - 1] - dist_from_f_mid)) print(sum_dist) if sum_dist[len(sum_dist) - 1] <= 1: print("<1") pass elif sum_dist[len(sum_dist) - 1] > 1: print(">1") pivot_left(p_l_tf) dist_from_right.append(s.r_sensor()) sum_dist.append(abs(dist_from_right[len(dist_from_right) - 1] - dist_from_f_mid)) if sum_dist[len(sum_dist) - 1] <= 1: print(sum_dist) print(dist_from_right) pass elif 1 < sum_dist[len(sum_dist) - 1] <= sum_dist[len(sum_dist) - 2]: pivot_left(p_l_tf) dist_from_right.append(s.r_sensor()) sum_dist.append(abs(dist_from_right[len(dist_from_right) - 1] - dist_from_f_mid)) if sum_dist[len(sum_dist) - 1] <= 1 or sum_dist[len(sum_dist) - 1] < sum_dist[len(sum_dist) - 2]: print(sum_dist) print(dist_from_right) pass elif sum_dist[len(sum_dist) - 1] > sum_dist[len(sum_dist) - 2]: pivot_right(p_r_tf) elif sum_dist[len(sum_dist) - 1] > sum_dist[len(sum_dist) - 2]: pivot_right(p_r_tf) dist_from_right.append(s.r_sensor()) sum_dist.append(abs(dist_from_right[len(dist_from_right) - 1] - dist_from_f_mid)) if sum_dist[len(sum_dist) - 1] <= 1: pass elif sum_dist[len(sum_dist) - 1] > 1: pivot_right(p_r_tf) dist_from_right.append(s.r_sensor()) sum_dist.append(abs(dist_from_right[len(dist_from_right) - 1] - dist_from_f_mid)) except Exception as e: print('error in 90 deg left') print(e) cleanup()
def start_sensor_reader(): global RELEASE_CHAMBER_COUNT, LEFT_CHAMBER_COUNT, RIGHT_CHAMBER_COUNT RELEASE_CHAMBER_COUNT = 0 LEFT_CHAMBER_COUNT = 0 RIGHT_CHAMBER_COUNT = 0 left_dc_sensor = Sensor.init_sensor("left_decision_chamber","55739323237351D091A1") right_dc_sensor = Sensor.init_sensor("right_decision_chamber",'5573932323735121C051') release_c_sensor = Sensor.init_sensor("release_chamber",'55739323237351310101') count_down('Open gates in', 5, '', False) #delete after demo EXPERIMENT_PREP_TIME t_end = time.time() + 60 * .5 #.5 is 30 seconds print(experiment_header) print("TEST HAS BEGUN") left_dc_voltage = 0.00 right_dc_voltage = 0.00 release_chamber_voltage = 0.00 while time.time() <= t_end: if(left_dc_sensor.take_reading() < (left_dc_sensor.avg_voltage - 0.03)): LEFT_CHAMBER_COUNT += 1 pline("!!!DETECTED!!!", right_dc_sensor.take_reading(),release_c_sensor.take_reading()) while(((left_dc_sensor.take_reading()) < (left_dc_sensor.avg_voltage - 0.03)) and (time.time() <= t_end)): pline("- not counted -", right_dc_sensor.take_reading(),release_c_sensor.take_reading()) if(right_dc_sensor.take_reading() < (right_dc_sensor.avg_voltage - 0.03)): RIGHT_CHAMBER_COUNT += 1 pline(left_dc_sensor.take_reading(),"!!!DETECTED!!!", release_c_sensor.take_reading()) while((right_dc_sensor.take_reading() < (right_dc_sensor.avg_voltage - 0.03)) and (time.time() <= t_end)): pline(left_dc_sensor.take_reading(),"- not counted -",release_c_sensor.take_reading()) if(release_c_sensor.take_reading() < (release_c_sensor.avg_voltage - 0.03)): RELEASE_CHAMBER_COUNT +=1 pline(left_dc_sensor.take_reading(), right_dc_sensor.take_reading(),("!!!DETECTED!!!" + str(release_c_sensor.voltage))) while((release_c_sensor.take_reading() < (release_c_sensor.avg_voltage - 0.03)) and (time.time() <= t_end)): pline(left_dc_sensor.take_reading(),right_dc_sensor.take_reading(), "- not counted -") pline(left_dc_sensor.take_reading(),right_dc_sensor.take_reading() ,release_c_sensor.take_reading()) print("!!!!!!!!!!!!!!DONE!!!!!!!!!!!!!!\n") calculate_stats()
def __init__(self, initScript=None): super(ConvReactor, self).__init__(initScript) self.isoRxn = IsothermalConvReactor() self.AddUnitOperation(self.isoRxn, 'IsoRxn') self.isoRxn.containerUnitOp = self self.heater = Heater.Heater() self.AddUnitOperation(self.heater, 'RxnHeater') self.baln = Balance.BalanceOp() self.AddUnitOperation(self.baln, 'EneBalance') #Initialize with one energy In and Out as a default #The parameter QEXOTHERMIC_ISPOS_PAR will set the missing energy port self.baln.SetParameterValue(NUSTIN_PAR + Balance.S_ENE, 1) self.baln.SetParameterValue(NUSTOUT_PAR + Balance.S_ENE, 1) self.baln.SetParameterValue(Balance.BALANCETYPE_PAR, Balance.ENERGY_BALANCE) # to create an Energy Out port for export self.balEneSensor = Sensor.EnergySensor() self.AddUnitOperation(self.balEneSensor, 'BalEneSensor') self.eneSensor = Sensor.EnergySensor() self.AddUnitOperation(self.eneSensor, 'EneSensor') self.eneStream = Stream.Stream_Energy() self.AddUnitOperation(self.eneStream, 'EneStream') # connect the child unit ops self.ConnectPorts('IsoRxn', OUT_PORT + 'Q', 'EneBalance', IN_PORT + 'Q0') self.ConnectPorts('RxnHeater', IN_PORT + 'Q', 'EneBalance', OUT_PORT + 'Q0') self.ConnectPorts('IsoRxn', OUT_PORT, 'RxnHeater', IN_PORT) self.ConnectPorts('EneSensor', OUT_PORT, 'EneStream', IN_PORT) # self.ConnectPorts('BalEneSensor', SIG_PORT, 'EneSensor', SIG_PORT) # borrow child ports self.BorrowChildPort(self.eneStream.GetPort(OUT_PORT), OUT_PORT + 'Q') self.BorrowChildPort(self.isoRxn.GetPort(IN_PORT), IN_PORT) self.BorrowChildPort(self.heater.GetPort(OUT_PORT), OUT_PORT) self.BorrowChildPort(self.heater.GetPort(DELTAP_PORT), DELTAP_PORT) self.SetParameterValue(NURXN_PAR, 0) self.SetParameterValue(SIMULTANEOUSRXN_PAR, 1) self.SetParameterValue(QEXOTHERMIC_ISPOS_PAR, 1)
def ninty_deg_right(): try: # print(" In 90 Deg to the Right") dist_from_f_mid = round(s.m_sensor()) f2 = [] pivot_right(f_p_r_tf) f2.append(s.m_r_sensor()) while not s.f_sensor(): if not first_func_for_90dr(f2): break if not s.f_sensor(): second_funck_for_90dr(dist_from_f_mid) except Exception as e: print('error in pivot 90 deg right') print(e) cleanup()
def main(): # Initialization ctrl = ControlPublisher() sensor = Sensor() particles = init_particles(C.NPARTICLES) plotter = ProcessPlotter() plotter.settings(DOPLOT, FREQUENCY, SAVE) for i, t in enumerate(C.T): ctrlData = ctrl.read(t) if (ctrlData.speed != 0): # Prediction for particle in particles: particle.predictACFRu(ctrlData) # Measurement z = FrontEnd.filter( sensor.read(t, C.T[i + 1]) ) # TODO: In case speed at last time is different 0 would crash if size(z): # Data associations for particle in particles: particle.data_associateNN(z) # Known map features for particle in particles: # Sample from optimal proposal distribution if particle.zf.size: particle.sample_proposaluf( ) #Updates state estimation taking into account the measurements (FastSLAM 2.0) # Map update particle.feature_updateu() particles = resample_particles(particles, C.NEFFECTIVE) plotter.update(get_message(particles, z, t)) # When new feautres are observed, augment it ot the map for particle in particles: if particle.zn.size: particle.augment_map() plotter.terminate()
def __init__(self, filename): fcfg = open(self.cfgdir + filename) self.name = filename[:-4] self.port = fcfg.readline().rstrip() self.ser = serial.Serial(self.port, timeout=1) numSensors = int(fcfg.readline().rstrip()) for i in range(0, numSensors): self.sensors.append(Sensor.Sensor(fcfg.readline().rstrip()))
def wt_feedback(dist1, dist3): try: # print("wt fb") if not s.f_sensor(): dist2 = s.l_sensor() dist4 = s.r_sensor() diff = abs(dist2 - dist1) diff2 = abs(dist4 - dist3) if diff < 15: wt_check_on_left(dist1, dist2, diff) elif diff >= 15: wt_check_on_right(dist2, dist3, dist4, diff2) except Exception as e: gpio.cleanup() print("error in wtfeedback") print(e)
def Main(): #while(True): SensorInfo = Sensor.Main() # return number if (SensorInfo != 0): # do anything that needs to happen for processing the data Type = wasteType(SensorInfo) DataRecording(Type, SensorInfo) if (Type != "invalid"): MechElements.Main(SensorInfo)
def fillDemo(self): #TODO: Fill DB with some simple data metric = Sensor.Sensor() values = metric.getRate("T1_FNAL_MSS", "T1_FNAL_Buffer") name = "Rate" query = """select nvl(sum(done_bytes),0)/1048576/3600 rate from t_history_link_events where timebin >= (sysdate - TO_DATE('01.01.1970:00:00:00','DD.MM.YYYY:HH24:MI:SS'))*24*60*60 - 7200 and to_node = 'T1_FNAL_MSS' and from_node = 'T1_FNAL_Buffer'""" fillDB = FillSensorDB.FillSensorDB() fillDB.Fill_SQL_METRIC(query, name, values) return
def setup(): global lost global sensor # reset the lost variable lost = False # create the sensor sensor = Sensor.Sensor(rangeOfSensor, x, y, dy, maxRange)
def __init__(self, initScript=None): super(EquilibriumReactor, self).__init__(initScript) self.itnRxn = InternalEqmReactor() self.AddUnitOperation(self.itnRxn, 'itnRxn') self.itnRxn.containerUnitOp = self self.baln = Balance.BalanceOp() self.AddUnitOperation(self.baln, 'EneBalance') self.baln.SetParameterValue(NUSTIN_PAR + Balance.S_ENE, 1) self.baln.SetParameterValue(Balance.BALANCETYPE_PAR, Balance.ENERGY_BALANCE) # to create an Energy Out port for export self.balEneSensor = Sensor.EnergySensor() self.AddUnitOperation(self.balEneSensor, 'BalEneSensor') self.eneSensor = Sensor.EnergySensor() self.AddUnitOperation(self.eneSensor, 'EneSensor') self.eneStream = Stream.Stream_Energy() self.AddUnitOperation(self.eneStream, 'EneStream') # connect the child unit ops self.ConnectPorts('itnRxn', OUT_PORT + 'Q', 'EneBalance', IN_PORT + 'Q0') self.ConnectPorts('EneSensor', OUT_PORT, 'EneStream', IN_PORT) self.ConnectPorts('BalEneSensor', SIG_PORT, 'EneSensor', SIG_PORT) # borrow child ports self.BorrowChildPort(self.eneStream.GetPort(OUT_PORT), OUT_PORT + 'Q') self.BorrowChildPort(self.itnRxn.GetPort(IN_PORT), IN_PORT) self.BorrowChildPort(self.itnRxn.GetPort(OUT_PORT), OUT_PORT) self.BorrowChildPort(self.itnRxn.GetPort(DELTAP_PORT), DELTAP_PORT) self.SetParameterValue(NURXN_PAR, 0) self.SetParameterValue(CALCOPTION_PAR, 1) #use table to correlate constant self.SetParameterValue(CONSTBASIS_PAR, 1) #use partial pressure to calculate constant self.SetParameterValue( BASISPUNIT_PAR, 'atm') #VMG pressure unit when partial pressure as basis self.SetParameterValue(QEXOTHERMIC_ISPOS_PAR, 1)
def __init__(self, sensornid=-1, sensornodename=None, isalive=False, jsonPayload=None): self.sensorNodeId = sensornid self.sensors = dict() self.sensorNodeName = sensornodename self.is_alive = isalive if jsonPayload != None: try: json2dict = json.loads(jsonPayload) tid = int(json2dict["TrainID"]) self.is_alive = False if json2dict["Status"] == "On": self.is_alive = True self.sensorNodeId = int(json2dict["SensorID"]) self.sensorNodeName = json2dict["SensorName"] # process sensors modules if self.sensorNodeName == "TempHum": sensorModules = dict() sname = "temp" smeasurement = "celsius" svalue = json2dict["temp"] sensorModules[sname] = Sensor.Sensor( sname, smeasurement, svalue) sname = "hum" smeasurement = "%" svalue = json2dict["hum"] sensorModules[sname] = Sensor.Sensor( sname, smeasurement, svalue) self.setCurrentStatus(self.is_alive, sensorModules) #return self else: print "Unrecognized sensor node: ", self.sensorNodeName except Exception, e: print "Error at SensorNode.init().", str(e)
def __init__(self, wall_map, x, y, canvas): Robot.__init__(self) self._sonar = Sensor("PORT_4", "sonar") self._map = None if wall_map is not None: self._map = wall_map.get_walls() self._particles = [ParticleMCL(x, y, 0, (1.0/NUMBER_OF_PARTICLES), self._map) for i in range(NUMBER_OF_PARTICLES)] BrickPiSetupSensors() if canvas is not None: self._canvas = canvas self._not_sampling = 0
def add_sensor_to_monitor_post(): sensor_uuid = request.form.get("uuid") delta_time = request.form.get("delta_time") res = Sensor.api_add_sensor_to_monitor({ 'uuid': sensor_uuid, 'delta_time': delta_time }) if res: Response(json.dumps(res[0], indent=2, sort_keys=True), mimetype='application/json'), res[1] return redirect(url_for('uuid_management', uuid=sensor_uuid))
class SensorTask(): def __init__(self, lock, bd_addr): self.sensor = Sensor(lock, bd_addr) self.bd_addr = bd_addr self.persist = SensorDataPersist() self.emailer = EmailNotifier() self.db = MongoInstanceBuilder().get_db() def _load(self, value): self.data = json.loads(value) self.data['date'] = datetime.datetime.now().strftime( "%Y-%m-%d %H:%M:%S") self.data['sensorid'] = self.bd_addr print self.data return self.data def execute(self): self.sensor.start() self.sensor.join() value = self.sensor.value if value is not None: self.persist.write(self._load(value)) self._notify_by_mail(value) def _notify_by_mail(self, value): sensor = self.db.sensors.find_one({"_id": self.bd_addr}) try: _subscribers = sensor["subscribers"] for _subscriber in _subscribers: if self._is_sensor_a_plant(sensor): value_to_check = self.data["moisture"] if float(value_to_check) < float(sensor["warninglevel"]): print("Notify by mail " + _subscriber["mail"] + value_to_check) emailer = EmailPlantNotifier() emailer.notify(_subscriber) except: _subscribers = [] def _is_sensor_a_plant(self, sensor): return sensor["type"] == "plant"
def measureHeight(): while True: try: print("Initialising") x = Sensor.Sensor(7, 6) #Second Pin must be PWM for i in range(5): print(x.measure()) h = x.measuremore(25) #returns distance in cm print "Sane Measurement", h return h except TypeError: continue
def __init__(self, initialMap): # (x, y) = (1, 1) because we assume that the robot is 3 x 3 and it is placed right # in the middle of the 3 x 3 box self.x = 1 self.y = 1 self.orientation = RobotOrientation.FRONT # For sensor reading simplicity, the POSITION of the sensor is as if the sensor is placed in the front and then rotated self.sensors = [ Sensor.Sensor(SensorPosition.FRONT_SENSOR, SensorRange.SHORT_SENSOR, (-1, 1)), Sensor.Sensor(SensorPosition.FRONT_SENSOR, SensorRange.SHORT_SENSOR, (0, 1)), Sensor.Sensor(SensorPosition.FRONT_SENSOR, SensorRange.SHORT_SENSOR, (1, 1)), # Sensor.Sensor(SensorPosition.LEFT_SENSOR, SensorRange.SHORT_SENSOR, (-1, 1), -1), # Sensor.Sensor(SensorPosition.LEFT_SENSOR, SensorRange.SHORT_SENSOR, (0, 1), -1), # Sensor.Sensor(SensorPosition.LEFT_SENSOR, SensorRange.SHORT_SENSOR, (1, 1), -1) ] self.mapKnowledge = initialMap
def stations_sensors_info(station_id): url_string = 'http://api.gios.gov.pl/pjp-api/rest/station/sensors/{}' with urllib.request.urlopen(url_string.format(station_id)) as url: json_string = str(url.read().decode()) result = Sensor.sensor_from_dict(json.loads(json_string)) station_sensors_info = [] for r in result: station_sensors_info.append(r.param.param_name) return jsonify(station_sensors_info)
class Robot: def __init__(self): self.moteur = RobotMotor(5.5, 14.0, OUTPUT_A, OUTPUT_D) self.sensor = Sensor() self.ts = TouchSensor() self.is_running = False def start(self): while (self.ts.value()): if self.sensor.get_distance() < 2: self.moteur.stop() else: self.moteur.go_forwards()
def dn(flag, row, col, length, width, dl=False): # print(" In down Navigation") try: if s.m_sensor() < 15: reverse() delay(move_delay) if not dl: if flag == 'r': ninty_deg_right() forwardFeed(flag, row, col, length, width) if s.m_sensor() < 10: reverse(0.2) ninty_deg_right() elif flag == 'l': ninty_deg_left() forwardFeed(flag, row, col, length, width) if s.m_sensor() < 10: reverse(0.2) ninty_deg_left() return row + 1 else: if flag == 'r': ninty_deg_right() forwardFeed(flag, row, col, length, width) if s.m_sensor() < 10: reverse(0.2) ninty_deg_left() elif flag == 'l': ninty_deg_left() forwardFeed(flag, row, col, length, width) if s.m_sensor() < 10: reverse(0.2) ninty_deg_right() return row + 1 except Exception as e: print("error in down nav") print(e) gpio.cleanup()
def can_move_forward(): try: if s.m_sensor() > 25: if 9 < s.m_l_sensor() < 25: print("issue: ", s.m_l_sensor()) pivot_right(p_r_tf+0.03) elif s.m_l_sensor() <= 9: print("issue: ", s.m_l_sensor()) pivot_right(p_r_tf+0.08) elif 9 < s.m_r_sensor() < 25: print("issue: ", s.m_r_sensor()) pivot_left(p_l_tf+0.03) elif s.m_r_sensor() <= 9: print("issue: ", s.m_r_sensor()) pivot_left(p_l_tf+0.08) except Exception as e: print('error in can_move_forward') print(e) cleanup()
def main(): logging.basicConfig(format='%(asctime)s|%(levelname)s|%(message)s', datefmt='%H:%M:%S', level=logging.INFO) serial_ports = serial.tools.list_ports.comports() boards = [] for port in serial_ports: if (port.vid == 1240 and port.pid == 10): boards.append(port.device) print("Select which device to use: " + str(boards)) COM_port = input(">") sensor = Sensor.Sensor(COM_port) tui = TUI(sensor) tui.show()
def setUp(self): if os.path.exists(LogfileParser.logfile): os.remove(LogfileParser.logfile) self.sensor = Sensor.Sensor( name="sensor", pagesToWatch=["http://heise.de/tr", "https://www.google.com"]) self.sensor.pauseBetweenDuties = 1 thread = Thread(name="Runnable %s" % self.sensor.name, target=self.sensor.start) shutil.rmtree(self.sensor.outputdir, ignore_errors=True) thread.start() time.sleep(2.5) self.sensor.stop()
def main(): parser = OptionParser(usage="usage: %prog [options] ", version="%prog 1.0") parser.add_option("-i", "--init", action="store_true", dest="init", default=False, help=u"初始化数据库") parser.add_option("-s", "--string", action="store", dest="search_string", default="", help=u"搜索字符串") (options, args) = parser.parse_args() if options.init: init() exit(0) Configuration.load('debug.yml') DBMS.create_table(Configuration.db_path) Sensor.sensor('20170225') Spider.spider(file_date='20170225', log_type='postran') conditions = parse_search_string(options.search_string) conditions["logType"] = 'postran' finder_result = Finder.finder(conditions) #finder_result = Finder.finder({'logType': 'postran', 'FileDate': '20140715', 'rrn': '141960021923', 'amount': '', # 'MrchId': ''}) Filter.filter_by_call(finder_result)
def __init__(self, lock): self._lock = lock self._curr_pose = PoseStamped() self.sense = Sensor.sensor() self.detect_data = LaserScan() self.height_data = LaserScan() rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._curr_pose_callback) self._pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) self._pose_msg = PoseStamped() self._pose_state = "posctr" self.set_state("posctr") ps = [0.0, 0.0, 2.0] self.set_msg(ps)
def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 55, dps = 15.8): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.distancePerSecond = dps self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0
def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 100): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0 print "Left Speed: ", self.leftSpeed, "\nRight Speed: ", self.rightSpeed
def initRandom(self): self._width = rdm.randint(5, 10) del self._targets[:] del self._sensors[:] for i in range(rdm.randint(5, 10)): self._targets.append(Target()) self._targets[i].initRandom(self._width) for t in self._targets: s = Sensor() s.initRandom(self._width) while self.sensorCoversTarget(s, t) == False: s = Sensor() s.initRandom(self._width) self._sensors.append(s)
def main(): global numOfAcq global linenr global tsc tup = [] tup = Sensor.mock(tsc, linenr) if tsc >= 2: tsc -= 1 Parser.parse(tsc) cmd = "cd DATABASE; ./bmi.sh" p = subprocess.call(cmd, shell=True) numOfAcq += 1 linenr = tup[0] tsc = tup[1] print("DATA HAS BEEN ACQUIRED " + str(numOfAcq) + " TIMES\n") if not (linenr == -1): threading.Timer(0, main).start() # threads in seconds else: return
class RobotMCL(Robot): def __init__(self, wall_map, x, y, canvas): Robot.__init__(self) self._sonar = Sensor("PORT_4", "sonar") self._map = None if wall_map is not None: self._map = wall_map.get_walls() self._particles = [ParticleMCL(x, y, 0, (1.0/NUMBER_OF_PARTICLES), self._map) for i in range(NUMBER_OF_PARTICLES)] BrickPiSetupSensors() if canvas is not None: self._canvas = canvas self._not_sampling = 0 def get_wall_map(self): return self._map def draw_particles(self): self._canvas.drawParticles([p.to_tuple() for p in self._particles]) def sample_particle(self, total_weight): sample = random.uniform(0,total_weight) temp_weight = 0 for p in self._particles: temp_weight += p.get_weight() if (temp_weight >= sample): return p def get_actual_sonar_value(self): readings = [] time.sleep(0.2) reading_time = time.time() while(len(readings) <= 10 and time.time() - reading_time < 0.5): value = self._sonar.get_value() if (value < 255): readings.append(value) readings.append(self._sonar.get_value()) mode = Counter(readings).most_common(1)[0][0] return mode + SONAR_DIFFERENCE def resample_particles(self): total_weight = 0 sonar_reading = self.get_actual_sonar_value() print("SONAR!: ", sonar_reading) if sonar_reading < 255: for p in self._particles: total_weight += p.update_weight(sonar_reading) new_particles = [] for p in self._particles: new_p = self.sample_particle(total_weight) new_particles.append(ParticleMCL(new_p.get_x(), new_p.get_y(), new_p.get_theta(), (1.0/NUMBER_OF_PARTICLES), self._map)) self._particles = new_particles else: globals.BIG_ANGLE = True def navigateToWaypoint(self, theta, distance): BrickPiUpdateValues() self.turn(theta) time.sleep(0.1) self.forward(distance) self.print_stuff() angle = self.angle_to_wall() resampled = True print("angle to the wall: ", angle) if (angle < 15): globals.BIG_ANGLE = False self.resample_particles() else: globals.BIG_ANGLE = True #self.resample_particles() self._not_sampling += 1 resampled = False self.print_stuff() return resampled def print_stuff(self): (x_curr, y_curr, theta_curr) = self.get_current_position() print("x_curr: ", x_curr, "y_curr: ", y_curr, "theta: ", theta_curr) def navigate_to_way_point_a_bit(self, x, y, old_pos=None): if(not old_pos): (x_curr, y_curr, theta_curr) = self.get_current_position() else: (x_curr, y_curr, theta_curr) = old_pos theta = self.get_degrees_to_turn(x_curr, y_curr, theta_curr, x, y) print("x,y,theta: ", (x_curr, y_curr, theta_curr) ) distance = self.get_distance_to_move(x_curr, y_curr, x, y) if distance - CYCLE_LENGTH <= 7: new_distance = distance else: new_distance = CYCLE_LENGTH print("Moving distance: ", new_distance, "Rotating degrees", theta) resampled = self.navigateToWaypoint(theta, new_distance) if(distance - CYCLE_LENGTH > 7): old_pos = None if(not resampled or globals.BIG_ANGLE or globals.BAD): old_pos = (x_curr+new_distance*math.cos(math.radians(theta+theta_curr)), y_curr+new_distance*math.sin(math.radians(theta+theta_curr)), theta_curr + theta) self.navigate_to_way_point_a_bit(x, y, old_pos) else: self.resample_particles() print("NEW POINT") def angle_to_wall(self): (x_curr, y_curr, theta_curr) = self.get_current_position() (Ax, Ay, Bx, By) = self.get_current_wall() top = math.cos(math.radians(theta_curr)) * (Ay - By) + math.sin(math.radians(theta_curr)) * (Bx - Ax) bottom = math.sqrt(math.pow(Ay - By, 2) + math.pow(Bx - Ax, 2)) return math.degrees(math.acos(top / bottom)) def get_current_wall(self): wall_list = map(self.find_wall, self._particles) return Counter(wall_list).most_common(1)[0][0] def find_wall(self, particle): return particle.calc_min_distance_to_wall()[1]
import RPi.GPIO as GPIO import DButil as dbu import Sensor import OutputDevice as od led_port = 20 buzzer_port = 21 od.init(20) # LED od.init(21) # buzzer Sensor.init(23, 24) dbu.init('nosleep', 'mysql!nosleep1', 'db_no_sleep') dbu.test() try: while True : isOnoff = 0 distance = Sensor.getDistance() if(distance >= 20.0): od.LEDon(led_port) od.buzz(buzzer_port, 10.0, 1.0) isOnoff = 1 else: od.LEDoff(led_port) print "distance : " + str(distance) sql = "INSERT INTO tb_timetable(distance, onoff) VALUE(" + str(distance) + ", " + str(isOnoff) + ");" dbu.exeSql(sql) except KeyboardInterrupt:
class Move: TOO_CLOSE = "Too Close"; def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 55, dps = 15.8): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.distancePerSecond = dps self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0 def checkBoundary(self,cmSent): # situation 1 # we send something and we find out that the robot is going to hit the wall if it continues, meaning the distance is greater #than the closest thing away, that the worst possible thing, so lets check for that first. #if the desired distance is greater or equal to the distance away from the closest obstacle cmAwayFromWall = self.sensor.getDistance() if (cmSent > cmAwayFromWall): return "STOP" elif (cmSent > (cmAwayFromWall - 10)): return "STOP" elif (cmSent < (cmAwayFromWall - 10)): return "GOOD" def normalize(self, val): scale = 0.5 / 100 speed = val * scale if val >= 0: speed += 0.5 return speed def turn(self,angle): self.movement.setMotorSpeed(pins.SERVO_RIGHT_MOTOR, 50) self.movement.setMotorSpeed(pins.SERVO_LEFT_MOTOR, -100) time1 = 0.9 if angle == 15: time1 = 0.11 if angle == 90: time1 = 0.84 else: a = angle % 360 time1 = a * 0.11 time.sleep(self.timeSpin)#needs to be replaced with time1 after it is calibrated self.stop() def forward(self, speed = 100): ''' self.left.set_normalized(1.0) time.sleep(0.01) print self.rightSpeed self.right.set_normalized(-self.rightSpeed) ''' self.movement.setMotorSpeed(pins.SERVO_LEFT_MOTOR,100) time.sleep(0.01) self.movement.setMotorSpeed(pins.SERVO_RIGHT_MOTOR, self.rightSpeed) def move(self,distance): status = True result = self.checkBoundary(distance) cmAwayFromWall = self.sensor.getDistance() print cmAwayFromWall if (result == "STOP"): distanceMoved = 0 self.movement.stop() return distanceMoved elif (result =="GOOD"): self.forward(100) time.sleep(distance/self.distancePerSecond) # return status #account for the 5 degrees a second/ every 12 cm of curvature to the left #anglesToTurn = (distance/12) * 5 #spinTime = anglesToTurn*0.0053763408602 # degrees per second # telling the robot to spin back since it turns left on its own #self.timedSpin(spinTime,'right') self.movement.stop() distanceMoved = cmAwayFromWall - self.sensor.getDistance() return distanceMoved def timedSpin(self,spinTime,direction): if (direction == 'left'): self.movement.rotate_left(); elif (direction == 'right'): self.movement.rotate_right() time.sleep(spinTime) self.movement.stop() def turnMe(self,angle): # 90 degrees of rotation comes to approx 0.66 seconds, 2 seconds = 270* of rotation on a smooth surface angle = float(angle) if (angle >0): direction = 'right' else: direction = 'left' angle = abs(angle) if angle == 15: self.timeSpin = 0.0067 elif angle == 90 or angle == -90: self.timeSpin = 0.0083 else: self.timeSpin = 0.0067 print self.timeSpin spinTime = angle * self.timeSpin # degrees per second 0.0053763408602 self.timedSpin(spinTime,direction) def scanHallway(self): start = self.sensor.getDistance() self.sensor.getSensor() print start t1 = time.time() self.forward() while True: try: r = self.sensor.getSensor('r') l = self.sensor.getSensor('l') print r,l if r + l + 8 > 30: self.movement.stop() break except Exception as e: print e t2 = time.time() end = self.sensor.getDistance() distanceMoved = 0 if start >= 100: distanceMoved = (t2-t1) * self.distancePerSecond else: distanceMoved = start - end print end return (distanceMoved, (t2-t1) * self.distancePerSecond) def findDoor(self, side = 'r', distance = 10, maxDistance = 90): start = self.sensor.getDistance() self.sensor.getSensor() print start t1 = time.time() self.forward() while True: try: r = self.sensor.getSensor(side) print r if r > 5 + distance: #if sensor reads a distance greater than 5 (a buffer) + distance its found the door self.movement.stop() break except Exception as e: print e t2 = time.time() distanceMoved = (t2-t1) * self.distancePerSecond if distanceMoved >= maxDistance: return -1 t2 = time.time() end = self.sensor.getDistance() distanceMoved = 0 if start >= 100: distanceMoved = (t2-t1) * self.distancePerSecond else: distanceMoved = start - end print end return (distanceMoved, (t2-t1) * self.distancePerSecond) def stop(self): self.left.set_normalized(-1) self.right.set_normalized(-1)
class Move: TOO_CLOSE = "Too Close"; def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 100): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0 print "Left Speed: ", self.leftSpeed, "\nRight Speed: ", self.rightSpeed def checkBoundary(self,cmSent): # situation 1 # we send something and we find out that the robot is going to hit the wall if it continues, meaning the distance is greater #than the closest thing away, that the worst possible thing, so lets check for that first. #if the desired distance is greater or equal to the distance away from the closest obstacle cmAwayFromWall = self.sensor.getDistance() if (cmSent > cmAwayFromWall): return "STOP" elif (cmSent > (cmAwayFromWall - 10)): return "STOP" elif (cmSent < (cmAwayFromWall - 10)): return "GOOD" def normalize(self, val): scale = 0.5 / 100 speed = val * scale if val >= 0: speed += 0.5 return speed def forward(self, speed = 100): self.left.set_normalized(1.0) time.sleep(0.01) self.right.set_normalized(-self.rightSpeed) def move(self,distance): status = True result = self.checkBoundary(distance) cmAwayFromWall = self.sensor.getDistance() if (result == "STOP"): distanceMoved = 0 self.movement.stop() return distanceMoved elif (result =="GOOD"): self.forward(100) time.sleep(distance/14.5) #account for the 5 degrees a second/ every 12 cm of curvature to the left distanceMoved = cmAwayFromWall - self.sensor.getDistance() self.movement.stop() return distanceMoved def timedSpin(self,spinTime,direction): if (direction == 'left'): self.movement.rotate_left(); elif (direction == 'right'): self.movement.rotate_right() time.sleep(spinTime) self.movement.stop() def turn(self,angle): # 90 degrees of rotation comes to approx 0.66 seconds, 2 seconds = 270* of rotation on a smooth surface angle = float(angle) if (angle >0): direction = 'right' else: direction = 'left' angle = abs(angle) ''' if angle == 15: self.timeSpin = 0.0036 if angle == 5: self.timeSpin = 0.0032 if angle == 45: self.timeSpin = 0.00455 if angle == 90: self.timeSpin = 0.00457 if angle == 180: self.timeSpin = 0.0048 ''' spinTime = angle * self.timeSpin # degrees per second 0.0053763408602 self.timedSpin(spinTime,direction) def stop(self): self.left.set_normalized(-1) self.right.set_normalized(-1)
#!/usr/bin/env python from Sender import * from Sensor import * import os os.system("sudo modprobe w1-gpio") os.system("sudo modprobe w1-therm") sender = Sender() for dirname, dirnames, filenames in os.walk("/sys/bus/w1/devices"): for subdirname in dirnames: if subdirname != "w1_bus_master1": sensor = Sensor(subdirname) temp = sensor.get_temp() response = sender.send(temp, subdirname) # debug print(temp) print(response.status_code)
class SensorRobot(Robot): def __init__(self): Robot.__init__(self) self._left_touch = Sensor("PORT_1", "touch") self._right_touch = Sensor("PORT_2", "touch") self._sonar = Sensor("PORT_4", "sonar") self.error = 0 BrickPiSetupSensors() #Send the properties of sensors to BrickPi def calibrate_sonar(self): while(True): actual_distance = self._sonar.get_value() print("actual_distance:", actual_distance) BrickPiUpdateValues() def forward_touch_sensor(self): while(True): self._motorA.set_speed(FORWARD_SPEED_A) self._motorB.set_speed(FORWARD_SPEED_B) left_bumped = self._left_touch.get_value() right_bumped = self._right_touch.get_value() if (left_bumped and right_bumped): self.forward_simple(-10) self.turn(90) elif (left_bumped): self.forward_simple(-10) self.turn(90) elif (right_bumped): self.forward_simple(-10) self.turn(-90) BrickPiUpdateValues() def forward_sonar(self, distance): BrickPiUpdateValues() actual_distance = self._sonar.get_value() prev_values = [actual_distance] print("actual dist", actual_distance) while(True): if(actual_distance > distance): speed = K_SONAR * (actual_distance - distance) + MIN_SPEED elif abs(actual_distance - distance) <= 1: speed = 0 else: speed = K_SONAR * (actual_distance - distance) - MIN_SPEED print("sonar distance", self._sonar.get_value(), "actual dist", actual_distance, "speed: ", speed) self._motorA.set_speed(speed) self._motorB.set_speed(speed) prev_values = self.__get_prev_values(prev_values, self._sonar.get_value()) actual_distance = self.__get_mean_distance(prev_values) time.sleep(.01) BrickPiUpdateValues() def walking_test(self, distance): self.set_recover_speed_2(0) correction = 70 prev_time = time.time() while(time.time() - prev_time < 2): BrickPiUpdateValues() for i in range(0,10): print("TURN") self.set_recover_speed_2(correction) prev_time = time.time() while(time.time() - prev_time < 0.1): BrickPiUpdateValues() self.set_recover_speed_2(0) print("TURN") prev_time = time.time() while(time.time() - prev_time < 0.2): BrickPiUpdateValues() self.set_recover_speed_2(-1.2*correction) prev_time = time.time() while(time.time() - prev_time < 0.1): BrickPiUpdateValues() self.set_recover_speed_2(0) print("DONE") prev_time = time.time() while(time.time() - prev_time < 1): BrickPiUpdateValues() def wall_walking_2(self, distance): self.set_recover_speed_2(0) K = 7 while(True): BrickPiUpdateValues() curr_distance = self._sonar.get_value() print("curr_distance: ", curr_distance) diff_o = curr_distance - distance diff = math.copysign(min(abs(diff_o), 6),diff_o) print() if(diff != 0): correction = K * diff # if(curr_distance < 200): # if(diff != 0): # correction = K * diff self.set_recover_speed_2(1.5* correction) print("diff: ", diff, "correction: ", correction, "speeda: ", self._motorA.get_speed(), "speedb: ", self._motorB.get_speed()) prev_time = time.time() while(time.time() - prev_time < 0.2): BrickPiUpdateValues() self.set_recover_speed_2(0) prev_time = time.time() while(time.time() - prev_time < (0.2 * diff/6)): BrickPiUpdateValues() interval = 0.2 + 0.1 * diff/6 self.set_recover_speed_2(-1*correction) prev_time = time.time() while(time.time() - prev_time < interval): BrickPiUpdateValues() self.set_recover_speed_2(0) def set_recover_speed_2(self, speed): self._motorA.set_speed(FORWARD_SPEED_A - speed) self._motorB.set_speed(FORWARD_SPEED_B + speed) BrickPiUpdateValues() def wall_walking(self, distance): BrickPiUpdateValues() actual_distance = self._sonar.get_value() prev_values = [actual_distance] self.set_recover_speed(0) while(True): walk_time = time.time() speed = K_WALL * (actual_distance - distance) if(abs(speed) > 4): self.set_recover_speed(speed) print("speed_a", self._motorA.get_speed(), "speed_b", self._motorB.get_speed(), "distance", actual_distance, "sonar", self._sonar.get_value(), "speed", speed) print("START WAIT\n") while(time.time() - walk_time < 0.2): pass print("END WAIT\n") walk_time = time.time() self.set_recover_speed(-speed) print("speed_a", self._motorA.get_speed(), "speed_b", self._motorB.get_speed(), "distance", actual_distance, "sonar", self._sonar.get_value(), "speed", speed) while(time.time() - walk_time < 0.2): pass self.set_recover_speed(0) prev_values = self.__get_prev_values(prev_values, self._sonar.get_value()) actual_distance = prev_values[len(prev_values)-1] BrickPiUpdateValues() def set_recover_speed(self, speed): self._motorA.set_speed(FOLLOW_WALL_SPEED - speed) self._motorB.set_speed(FOLLOW_WALL_SPEED + speed) BrickPiUpdateValues() def __get_prev_values(self, prev_values, value): # if(value < prev_values[len(prev_values) -1] + DISTANCE_TRESHOLD): if(len(prev_values) >= ARRAY_LENGTH): prev_values.pop(0) prev_values.append(value) return prev_values def __get_mean_distance(self, prev_values): sorted_array = sorted(prev_values) if (len(prev_values) % 2 == 1): return sorted_array[len(prev_values)/2] else: mid = len(prev_values)/2 return (sorted_array[mid - 1] + sorted_array[mid])/2