def __init__(self):
self.w = wheels.wheels()
self.s = Sensors()
self.config = configuration.config()
self.w.setConfig(self.config)
self.s.setConfig(self.config)
self.s.calibration = True
def __init__(self):
super(Main, self).__init__()
## initialize sensors
self.sensorThread = False
self.radioThread = False
self.cameraThread = False
try:
self.sensorThread = Sensors()
except:
print("error initing sensor thread")
try:
pass
#self.cameraThread = Camera('Mini')
except:
print("error initializing camera thread")
try:
self.radioThread = Radio(self)
except:
print("error initing radio thread")
#self.numberGen = RandomThread()
self.daemon = True
self.fileLocation = str(os.getcwd()) + '/'
self.data_file = self.fileLocation + str(datetime.now().replace(microsecond=0))
print("initialized")
print(self.data_file)
def __init__(self):
self.filename = "config.txt"
with open(self.filename) as f:
config = f.read()
self.config_list = [
y for y in (x.strip() for x in config.splitlines()) if y
]
if self.find("onetouch") == "True":
self.onetouch = True
else:
self.onetouch = False
self.mysens = Sensors()
self.mouse = Plattform()
self.recived_data = None
self.count = [0, 0, 0, 0, 0]
self.controlboard = []
self.oldtouch = [False, False, False, False, False]
self.touch = [False, False, False, False, False]
self.sensibility = 0
self.actived = True
self.oldx = None
self.oldy = None
self.AS = [None, None, None] #X,Y,Z
self.AA = [None, None, None] # X,Y,Z
self.GS = [None, None, None] # X,Y,Z
self.GA = [None, None, None] # X,Y,Z
self.mysens.setGyroscopeAngolData(X=0.0, Y=0.0, Z=0.0)
self.mysens.setGyroscopeScalledData(X=0.0, Y=0.0, Z=0.0)
def main_loop(self):
while (self.game.count <= self.game.max_games):
#reseting game
self.game.reset()
self.init_distance()
self.sensors = Sensors(self.game.grid, self.game.player)
# game loop
while self.game.player.alive:
# give input to game
action = self.generate_action()
#action = 0
self.game.step(self.sensors, action)
# get observation
observation = [
self.sensors.obstacle_forward(),
self.sensors.obstacle_left(),
self.sensors.obstacle_right(),
self.get_food_distance(),
self.get_reward(), action
]
#training_data.append(observation)
##ANN
artificial_neural_network.train(observation)
# print("forward ", " left ", " right ", " food ", " action ", " reward ")
# print(observation)
#print(training_data)
#end of game loop
self.game.count = self.game.count + 1
def main():
for i in range(len(sys.argv)):
if sys.argv[i] == '-t':
if len(sys.argv) < i + 3:
print 'device and baud needed for option -t'
exit(1)
test(sys.argv[i + 1], int(sys.argv[i + 2]))
print 'Servo Server'
server = SignalKServer()
from sensors import Sensors
sensors = Sensors(server)
servo = Servo(server, sensors)
servo.max_current.set(10)
period = .1
start = lastt = time.time()
while True:
servo.poll()
sensors.poll()
if servo.controller.value != 'none':
print 'voltage:', servo.voltage.value, 'current', servo.current.value, 'ctrl temp', servo.controller_temp.value, 'motor temp', servo.motor_temp.value, 'rudder pos', sensors.rudder.angle.value, 'flags', servo.flags.strvalue(
)
#print servo.command.value, servo.speed.value, servo.windup
pass
server.HandleRequests()
dt = period - time.time() + lastt
if dt > 0 and dt < period:
time.sleep(dt)
lastt += period
else:
lastt = time.time()
def init():
gc.enable()
Status.init()
if not Config.init():
Config.factoryReset()
Config.init()
Wifi.init()
Leds.init()
Sensors.init()
Relays.init()
if Config.getParam("time", "ntp"):
i = 1
try:
while i < 5 and not ntpclient.settime(Config.getParam("time", "ntp"), Config.getParam("time", "offset")):
Status.log("Getting time from NTP... (%s)" % i)
i += 1
rtc = machine.RTC()
t = rtc.datetime()
except Exception:
t = utime.localtime(0)
else:
t = utime.localtime(0)
Status.log('Date: {:04d}.{:02d}.{:02d} {:02d}:{:02d}:{:02d}'.format(t[0], t[1], t[2], t[4], t[5], t[6]))
# Init timer
tim = machine.Timer(0)
tim.init(period=50, mode=machine.Timer.PERIODIC, callback=tick50mS())
def __init__(self,
arduino=None,
android=None,
fakeRun=False,
fakeMap=None,
stepsPerSec=1,
**kwargs):
"""
Constructor. Accepts attributes as kwargs.
Args:
fakeRun: set True if running simulation. Remember to give fake map as input. I.e: fakeMap = fakemap
fakeMap: set simulation map. If left empty, creates an empty arena.
pos: a 15x20 array. Contains None, 0 or 1 as values.
orientation: Centre of 3x3 start area. Default = [1,1]
map: Map object. Refer to Map.py
sensors: Sensors object. Refer to sensors.py
coordinator: Coordinator object. Refer to coordinator.py
"""
if fakeRun:
self.fakeRun = True
from sensors_fake import Sensors
self.sensors = Sensors(self, fakeMap) #fake sensors for simulation
self.coordinator.fakeRun = True
self.coordinator.stepsPerSec = stepsPerSec
self.imagefinder = Imagefinder(fakeRun=True)
elif arduino is None:
raise Exception("Real run requires arduino to be present")
elif android is None:
raise Exception("Real run requires arduino to be present")
else:
from sensors import Sensors
self.android = android
self.sensors = Sensors(self, arduino)
self.coordinator.arduino = arduino
self.imagefinder = Imagefinder()
#update map
self.updatemap()
goalTiles = [ #set goal as explored
[12, 19],
[13, 19],
[14, 19],
[12, 18],
[13, 18],
[14, 18],
[12, 17],
[13, 17],
[14, 17],
]
valuelist = [0] * len(goalTiles)
self.map.setTiles(goalTiles, valuelist)
#initialise pathfinder
self.pathfinder = Pathfinder(self.map)
#initialise explorer
self.explorer = Explorer(self)
def __init__(self, car):
self.sensors = Sensors()
self.references = [-1, -1, -1, -1, -1]
self.car = car
if not self.is_calibrated():
self.calibration()
else:
self.load_calibration()
def _last(self, name):
data = []
Sensors.get_last(
lambda obj: obj['name'] == name,
data,
max_age=self._period)
if data:
return self._make(name, data[0]['value'], data[0]['timestamp'])
def __init__(self):
config = configparser.ConfigParser()
config_file = os.path.join(os.path.dirname(__file__), 'config.ini')
config.read(config_file)
client = InfluxDBClient.from_config_file(config_file)
self.write_api = client.write_api()
self.bucket = config['ha']['influx_bucket']
self.sensors = Sensors()
def setup(self):
self.logger.info("--------------------------------------")
self.logger.info(" Balloon Mission Computer V4.01 ")
self.logger.info("--------------------------------------")
self.logger.debug("setup")
# setup
with open('data/config.json') as fin:
self.config = json.load(fin)
self.images_dir = self.config["directories"][
"images"] if "directories" in self.config and "images" in self.config[
"directories"] else "./images"
self.tmp_dir = self.config["directories"][
"tmp"] if "directories" in self.config and "tmp" in self.config[
"directories"] else "./tmp"
if not os.path.exists(self.tmp_dir):
os.makedirs(self.tmp_dir)
if not os.path.exists(self.images_dir):
os.makedirs(self.images_dir)
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # Broadcom pin-numbering scheme
GPIO.setup(self.config['pins']['BUZZER'], GPIO.OUT)
GPIO.setup(self.config['pins']['LED1'], GPIO.OUT)
GPIO.setup(self.config['pins']['LED2'], GPIO.OUT)
GPIO.output(self.config['pins']['LED1'], GPIO.HIGH)
self.aprs = APRS(self.config['callsign'], self.config['ssid'],
"idoroseman.com")
self.modem = AFSK()
self.gps = Ublox()
self.gps.start()
self.radio = Dorji(self.config['pins'])
self.radio.init()
self.radio_q = queue.Queue()
self.radio_thread = threading.Thread(target=self.radio_worker)
self.radio_thread.start()
self.timers = Timers(self.config['timers'])
self.sensors = Sensors()
self.cam = Camera()
self.ssdv = SSDV(self.config['callsign'], self.config['ssid'])
self.sstv = SSTV()
self.webserver = WebServer()
self.radio_queue(self.config['frequencies']['APRS'],
'data/boatswain_whistle.wav')
for item in ["APRS", "APRS-META", "Imaging", 'Capture']:
self.timers.handle({item: self.config['timers'][item] > 0}, [])
self.timers.handle({"Buzzer": False}, [])
self.ledState = 1
self.imaging_counter = 1
self.state = "init"
self.min_alt = sys.maxsize
self.max_alt = 0
self.prev_alt = 0
self.send_bulltin()
GPIO.output(self.config['pins']['LED1'], GPIO.LOW)
def __init__(self, url, flightname, device_id=None, jwt_token=None):
self.url = '{}/flight/{}/telemetry?authorization={}'.format(
url, flightname, jwt_token)
self.timeout_seconds = 10
self.coords = []
self.imei = device_id
self.momsn = None
self.import_coordinates_list()
self.sensors = Sensors(self.coords)
def testCalculator_adds_and_gets_data(self):
sensors = Sensors()
data = sensors.poll()
calculator = server.Calculator()
calculator.add_datum(data)
self.assertIn(
"PEEP",
calculator.get_datum()["patient-0"],
"Fails to add data to and then get data from "
"the calculator.")
def __initVars(self):
"""Subscribers vars initialisation"""
self.sensors = Sensors()
self.trans_listener = tf.TransformListener()
self.sensors.add("trans", None)
self.old_trans = None
self.sensors.add("rot", None)
self.rate_pub_enabled = False
self.messager = mav_msgs.RateThrust()
self.collision = False
def _any(self, name):
data = []
Sensors.get_data(
lambda obj: obj['name'] == name,
data,
max_age=self._period)
if data:
for obj in data:
if obj['value']:
return self._make(name, True, obj['timestamp'])
def _average(self, name):
data = []
Sensors.get_data(
lambda obj: obj['name'] == name,
data,
max_age=self._period)
if data:
value = sum(elem['value'] for elem in data) / len(data)
timestamp = max(elem['timestamp'] for elem in data)
return self._make(name, value, timestamp)
def run_sensors(gapps, user_options):
"""
Utility method to publish to the gridappsdmock instead of going through
the gridappsd service process.
:param gapps:
:param user_options:
"""
sensors = Sensors(gapps, "read", "write", user_options)
for data in next_line():
sensors.on_simulation_message({}, deepcopy(data))
def __init__(self, x=0, y=0, theta=0):
self.x = x
self.y = y
self.theta = radians(theta)
self.velLin = 0
self.velAng = 0
self.distFront = 0
self.distRight = 0
self.distLeft = 0
self.sensors = Sensors()
self.control = False
def test_should_smooth_the_compass_bearing(self):
mock_bearing = PropertyMock(side_effect=[185.0,180])
mock_compass = Mock()
type(mock_compass).bearing = mock_bearing
sensors = Sensors(StubGPS(),self.windsensor,mock_compass,self.mock_time,self.exchange,self.logger,DEFAULT_CONFIG)
sensors._bearing = 190
self.exchange.publish(Event(EventName.tick))
self.exchange.publish(Event(EventName.tick))
self.assertEqual(sensors.compass_heading_instant,((190.0 + 185.0)/2 + 180.0)/2)
def initialize_threads(self):
""" Initializes io threads"""
super().initialize_threads()
self.dashboard_interval = int(self.ping_interval * 2)
self.topic_fastclock_pub = self.publish_topics[Global.FASTCLOCK]
self.topic_dashboard_pub = self.publish_topics[Global.DASHBOARD]
self.topic_node_pub = self.publish_topics[Global.NODE]
self.topic_ping_sub = self.subscribed_topics[Global.PING]
# print("!!! ping sub: "+self.topic_ping_sub)
self.topic_sensor_sub = self.subscribed_topics[Global.SENSOR]
self.topic_backup_sub = self.subscribed_topics[Global.BACKUP]
# print("!!! backup sub: "+self.topic_backup_sub)
if Global.CONFIG in self.config:
if Global.OPTIONS in self.config[Global.CONFIG]:
if Global.TIME in self.config[Global.CONFIG][Global.OPTIONS]:
if Global.FAST in self.config[Global.CONFIG][
Global.OPTIONS][Global.TIME]:
if Global.RATIO in self.config[Global.CONFIG][
Global.OPTIONS][Global.TIME][Global.FAST]:
self.fast_ratio = int(
self.config[Global.CONFIG][Global.OPTIONS][
Global.TIME][Global.FAST][Global.RATIO])
if Global.INTERVAL in self.config[Global.CONFIG][
Global.OPTIONS][Global.TIME][Global.FAST]:
self.fast_interval = int(
self.config[Global.CONFIG][Global.OPTIONS][
Global.TIME][Global.FAST][Global.INTERVAL])
if Global.PING in self.config[Global.CONFIG][Global.OPTIONS]:
self.ping_interval = self.config[Global.CONFIG][
Global.OPTIONS][Global.PING]
if Global.BACKUP in self.config[Global.CONFIG][Global.OPTIONS]:
self.backup_path = self.config[Global.CONFIG][
Global.OPTIONS][Global.BACKUP]
self.roster = Roster(self.log_queue,
file_path=Global.DATA + "/" + Global.ROSTER +
".json")
self.switches = Switches(self.log_queue,
file_path=Global.DATA + "/" +
Global.SWITCHES + ".json")
self.warrants = Warrants(self.log_queue,
file_path=Global.DATA + "/" +
Global.WARRANTS + ".json")
self.signals = Signals(self.log_queue,
file_path=Global.DATA + "/" + Global.SIGNALS +
".json")
self.layout = Layout(self.log_queue,
file_path=Global.DATA + "/" + Global.LAYOUT +
".json")
self.dashboard = Dashboard(self.log_queue,
file_path=Global.DATA + "/" +
Global.DASHBOARD + ".json")
self.sensors = Sensors(self.log_queue,
file_path=Global.DATA + "/" + Global.SENSORS +
".json")
class Logger:
def __init__(self):
config = configparser.ConfigParser()
config_file = os.path.join(os.path.dirname(__file__), 'config.ini')
config.read(config_file)
client = InfluxDBClient.from_config_file(config_file)
self.write_api = client.write_api()
self.bucket = config['ha']['influx_bucket']
self.sensors = Sensors()
def add_record(self, measurement, tag_key, tag_value, field_key,
field_value, nanos):
# https://v2.docs.influxdata.com/v2.0/reference/syntax/line-protocol/
if isinstance(field_value, str):
line = [
'{},{}={} {}="{}" {}'.format(measurement, tag_key, tag_value,
field_key, field_value, nanos)
]
else:
line = [
'{},{}={} {}={} {}'.format(measurement, tag_key, tag_value,
field_key, field_value, nanos)
]
self.write_api.write(bucket=self.bucket, record=line)
def run(self):
while True:
try:
dummy0 = self.sensors.get_dummy_sensor0()
self.add_record('sensor', 'location', 'home', 'dummy0', dummy0,
time.time_ns())
dummy1 = self.sensors.get_dummy_sensor1()
self.add_record('sensor', 'location', 'home', 'dummy1', dummy1,
time.time_ns())
p1_esmr5 = self.sensors.get_p1_esmr5()
nanos = time.time_ns()
for k, v in p1_esmr5.items():
self.add_record('p1', 'location', 'home', k, v, nanos)
except Exception as e:
self.add_record('exception', 'location', 'home', 'run', 1,
time.time_ns())
self.add_record('exception', 'location', 'home', 'msg', str(e),
time.time_ns())
continue
else:
self.add_record('exception', 'location', 'home', 'run', 0,
time.time_ns())
def main(self):
start_time = time.time()
cpu_usage = psutil.cpu_percent()
if getattr(sys, 'frozen', False):
configFile = Path(os.path.dirname(sys.executable) + "/config.yaml")
else:
configFile = Path(
os.path.dirname(os.path.abspath(__file__)) + "/config.yaml")
with open(str(configFile), 'r') as stream:
try:
config = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
self.URL = "http://" + str(config['server']['host']) + ":" + str(
config['server']['port'])
self.apikey = self.getApiKey()
textStorage = TextStorage(self.URL)
sensors = Sensors()
sensors.initializeSensors()
# Continuously loop
while True:
# Construct our weatherdata json object
cpu_usage = psutil.cpu_percent()
ram = psutil.virtual_memory()
ram_usage = ram.percent
weatherdata = sensors.getSensorData(self.apikey, cpu_usage,
ram_usage)
#Previous semester Posting Method
#time.sleep(4)
#try:
#r = requests.post(self.URL + '/api/weather', data = serv1)
#if (r.status_code == 200):
# textStorage.sendWeather()
# print("Sent: " + json.dumps(serv1))
# elif (r.status_code == 400):
# print("Invalid API key")
# Exception if unable to connect to server for the post request
#except (requests.exceptions.ConnectionError):
#print("Lost connection to server...storing data locally.")
# textStorage.storeWeather(serv1)
# pass
#The above commented code is the way previous semester posted to the server. We no longer need that because each individual Pi will not be posting anymore, and only Master will post.
#Because of that, each Pi needs to store their data locally and broadcast via LoRa to Master.
print("Lost connection to server...storing data locally.")
textStorage.storeWeather(weatherdata)
# Wait 3 seconds before restarting the loop
time.sleep(self.WAIT_TIME)
def main():
GPIO.setmode(GPIO.BOARD) #refference by pin number
GPIO.setwarnings(False)
#test_left_engine()
#test_right_engine()
#test_pwm()
sensors = Sensors()
while True:
print(sensors.check_sensors_state())
sleep(0.5)
pass
GPIO.cleanup()
def __init__(self):
pygame.init()
pygame.display.set_caption("AI Car game")
width = 1200
height = 900
self.screen = pygame.display.set_mode((width, height))
self.clock = pygame.time.Clock()
self.pad_group = pygame.sprite.RenderPlain(*pads2)
self.car = CarSprite('images/car.png', (100, 730))
self.sensors = Sensors(self.car.position, self.car.direction, self.pad_group)
self.ticks = 60
self.font = pygame.font.Font(None, 75)
self.exit = False
def __init__(self, xy, networks, agent_num, goal, boundary, training,
randomness, subsumption):
# store initial parameters
self.agent_num = agent_num
self.boundary = boundary
self.finished_completely = False
self.networks = networks
self.target = False
self.goal = goal
self.training = training
self.randomness = randomness
self.subsumption = subsumption
self.finished_counter = -1
self.finished = 0
# Initialize variables
self.stop = 0
self.restart = False
self.colliding = []
self.objects = []
self.batch_state_action_reward_tuples = []
self.network_input = []
self.box = [(-1, -1), (-1, -1), (-1, -1), (-1, -1)]
self.prev_moves = []
self.total = 0
self.won = False
self.interactive_dist = 0
self.turn = 0
self.start_reached = False
if self.networks.get_turn() < 2:
self.start_reached = True
self.next()
# Initialise agents parts and backup
self.parts = Parts(False, xy, self.networks.get_turn())
self.backup = Parts(True, None, 0)
self.parts.load_pair(self.backup)
self.backup.load_pair(self.parts)
self.array = self.parts.array
self.cog = self.parts.center_of_gravity(xy)
self.initial_position = self.cog[0]
self.last_score = 0.5
self.last_interactive = 0.5
# Initialise agents sensors
self.sensors = Sensors()
self.sensors.set_positions(self.array[0].get_pivot(),
self.array[2].get_pivot())
def test_should_smooth_the_compass_bearing(self):
mock_bearing = PropertyMock(side_effect=[185.0, 180])
mock_compass = Mock()
type(mock_compass).bearing = mock_bearing
sensors = Sensors(StubGPS(), self.windsensor, mock_compass,
self.mock_time, self.exchange, self.logger,
DEFAULT_CONFIG)
sensors._compass_smoothed = 190
self.exchange.publish(Event(EventName.tick))
self.exchange.publish(Event(EventName.tick))
self.assertEqual(sensors.compass_heading_smoothed,
((190.0 + 185.0) / 2 + 180.0) / 2)
def _get_experiment_metadata(date, sensorId):
sensors = Sensors(base="", station=STATION_NAME, sensor=sensorId)
exps = sensors.get_experiment(date)
experiment_md = []
for exp in exps:
curr_exp = {}
curr_exp["name"] = exp["name"]
curr_exp["start_date"] = exp["start_date"]
curr_exp["end_date"] = exp["end_date"]
curr_exp["url"] = exp["view_url"]
experiment_md.append(curr_exp)
return experiment_md
def main():
# Conexão com bd
connection = DbConnector(
cnn['host'],
cnn['port'],
cnn['database']
)
db = connection.connect()
sensor_db = db.sensores
# Criando lista de sensores
sensores_list = [
Sensors("Sensor 01"),
Sensors("Sensor 02"),
Sensors("Sensor 03")
]
# Definição das threads
thread01 = threading.Thread(
target=change_temp,
args=(
sensores_list[0],
2,
sensor_db
)
)
thread02 = threading.Thread(
target=change_temp,
args=(
sensores_list[1],
2,
sensor_db
)
)
thread03 = threading.Thread(
target=change_temp,
args=(
sensores_list[2],
2,
sensor_db
)
)
# Iniciando threads
thread01.start()
thread02.start()
thread03.start()
def __init__(self):
# set private attributs
self.__world_model__ = WorldModel()
self.__update_time__ = rospy.get_param('update_time', 0.9)
self.__max_trys__ = rospy.get_param('max_trys', 10)
self.__report__ = Report()
self.__sensors__ = Sensors(self.__report__)
self.__actuators__ = Actuators(self.__report__)
# main agent attributs
self.actions = Actions(self.__report__)
# start thread for world model update
thread.start_new_thread(self.__update_world_model__, ())
def __init__(self, gps=False, servo_port=SERVO_PORT):
# devices
self._gps = gps
self.windsensor = WindSensor(I2C(WINDSENSOR_I2C_ADDRESS))
self.compass = Compass(I2C(COMPASS_I2C_ADDRESS),
I2C(ACCELEROMETER_I2C_ADDRESS))
self.red_led = GpioWriter(17, os)
self.green_led = GpioWriter(18, os)
# Navigation
self.globe = Globe()
self.timer = Timer()
self.application_logger = self._rotating_logger(APPLICATION_NAME)
self.position_logger = self._rotating_logger("position")
self.exchange = Exchange(self.application_logger)
self.timeshift = TimeShift(self.exchange, self.timer.time)
self.event_source = EventSource(self.exchange, self.timer,
self.application_logger,
CONFIG['event source'])
self.sensors = Sensors(self.gps, self.windsensor, self.compass,
self.timer.time, self.exchange,
self.position_logger, CONFIG['sensors'])
self.gps_console_writer = GpsConsoleWriter(self.gps)
self.rudder_servo = Servo(serial.Serial(servo_port),
RUDDER_SERVO_CHANNEL, RUDDER_MIN_PULSE,
RUDDER_MIN_ANGLE, RUDDER_MAX_PULSE,
RUDDER_MAX_ANGLE)
self.steerer = Steerer(self.rudder_servo, self.application_logger,
CONFIG['steerer'])
self.helm = Helm(self.exchange, self.sensors, self.steerer,
self.application_logger, CONFIG['helm'])
self.course_steerer = CourseSteerer(self.sensors, self.helm,
self.timer,
CONFIG['course steerer'])
self.navigator = Navigator(self.sensors, self.globe, self.exchange,
self.application_logger,
CONFIG['navigator'])
self.self_test = SelfTest(self.red_led, self.green_led, self.timer,
self.rudder_servo, RUDDER_MIN_ANGLE,
RUDDER_MAX_ANGLE)
# Tracking
self.tracking_logger = self._rotating_logger("track")
self.tracking_sensors = Sensors(self.gps, self.windsensor,
self.compass, self.timer.time,
self.exchange, self.tracking_logger,
CONFIG['sensors'])
self.tracker = Tracker(self.tracking_logger, self.tracking_sensors,
self.timer)
def _get_sensor_fixed_metadata_url(sensorId):
"""
Assumes that the sensor fixed metadata stored in Clowder is authoritative
Ignore the fixed metadata in the JSON object and return the fixed metadata URL in Clowder.
"""
# TODO: Compare to known fixed metadata structure
# TODO; We only need this one -- duplicate method in metadata.py
# Get the dataset ID for the sensor by identifier
sensors = Sensors(base="", station=STATION_NAME, sensor=sensorId)
datasetid = sensors.get_fixed_datasetid_for_sensor()
properties = {}
properties["url"] = os.environ.get("CLOWDER_HOST","http://terraref.ncsa.illinois.edu/clowder/") + "api/datasets/" + datasetid + "/metadata.jsonld"
return properties
def main():
sensors = Sensors()
sensor_data = sensors.poll()
calculator = Calculator()
communicator = Communicator()
running = True
while running:
try:
calculator.add_datum(sensor_data)
communicator.publish_message(calculator.get_datum())
time.sleep(1.0)
except:
running = False
raise
def _get_sensor_fixed_metadata(sensorId, query_date):
# Get the json path for the sensor by identifier
sensors = Sensors(base="", station=STATION_NAME, sensor=sensorId)
jsonpath = sensors.get_fixed_jsonpath_for_sensor()
sensor_file = SENSOR_METADATA_CACHE + jsonpath
if os.path.exists(sensor_file):
with open(sensor_file, 'r') as sf:
md_json = json.load(sf)
if type(md_json) == list:
if type(md_json[0] == dict):
current_metadata = find_json_for_date(query_date, md_json)
return current_metadata
else:
# TODO: What should happen here?
return None
def __init__(self):
# # Check for correct input
# if isinstance(vehicle, Vehicle) is False:
# raise TypeError('Expected object of type Vehicle, got '+type(vehicle).__name__)
# Calling super class constructor
StoppableThread.__init__(self)
# These are the distances the drone passed in a certain direction.
# They are used to know if I need to try to pass the obstacle from another direction.
self.__right_distance = 0.0
self.__left_distance = 0.0
self.__up_distance = 0.0
# Always keep track of my last move in order to know better what to do in certain situations.
self.__last_move = None
# In case the drone need to pass an obstacle from above - keeping 2 flags. One for indicating its in the
# process of climbing, and another one to indicate it finished climbing and should now keep its altitude
# until passing the obstacle.
self.__keep_altitude = False
self.__climbing = False
# I want to maintain the drone's altitude for a short period of time before descending back to the
# original constant altitude, so for that I keep tracking of the time since it ascended.
self.__start_time_measure = 0.0
# In case of emergency, keeping a flag to order the drone to land.
self.__safety_protocol = False
# Other classes within the software for giving flight orders, get sensors data and
# calculate distances by geographic positioning system data.
# the __flight_commands & __location objects should get as a parameter the vehicle object
self.__flight_commands = FlightCommands() # FlightCommands(vehicle)
self.__sensors = Sensors()
self.__flight_data = FlightData() # FlightData(vehicle)
# Get the drone's location and height for further calculations.
self.__last_latitude = self.__flight_data.get_current_latitude()
self.__last_longitude = self.__flight_data.get_current_longitude()
self.__last_height = self.__flight_data.get_current_height()
# Counter and flag for being aware of a sequence of illegal sensor inputs in order to be aware of a
# malfunction in the system
self.__illegal_input_counter = 0
self.__last_input_legitimacy = True
self.__num_of_lines = int(self.__get_number_of_line_in_file()) # Delete when moving to a full functioning system.
# Initializing the sensors
if self.__sensors.connect() == -1:
raise ConnectionError('Cannot connect to sensors')
print("Connected Successfuly to Sensors!")
# Flag that indicates if the system is activated in order to give the user the knowledge if it should override
# other flight commands given to the drone. The flag is 'True' only for left/right/up maneuvers and False
# for all other cases including fixing the drone's altitude, since the altitude is fixed for 10 meters and
# any other running software within the drone shouldn't change its height.
self.__is_active = False
def __init__(self):
self.filename = "config.txt"
with open(self.filename) as f:
config = f.read()
self.config_list = [y for y in (x.strip() for x in config.splitlines()) if y]
if self.find("onetouch") == "True":
self.onetouch = True
else:
self.onetouch = False
self.mysens = Sensors()
self.mouse = Plattform()
self.recived_data = None
self.count = [0, 0, 0, 0,0]
self.controlboard = []
self.oldtouch = [False, False,False, False,False]
self.touch = [False, False,False, False,False]
self.sensibility = 0
self.actived = True
self.oldx = None
self.oldy = None
self.AS = [None,None,None] #X,Y,Z
self.AA = [None, None, None] # X,Y,Z
self.GS = [None, None, None] # X,Y,Z
self.GA = [None, None, None] # X,Y,Z
self.mysens.setGyroscopeAngolData(X = 0.0, Y = 0.0, Z = 0.0)
self.mysens.setGyroscopeScalledData(X=0.0, Y=0.0, Z=0.0)
def test_should_pass_through_wind_drection(self):
mock_angle = PropertyMock(return_value=10.0)
windsensor = Mock()
type(windsensor).angle = mock_angle
sensors = Sensors(StubGPS(), windsensor, self.compass, self.mock_time,
self.exchange, self.logger, DEFAULT_CONFIG)
self.assertEqual(sensors.wind_direction_relative_instant, 10.0)
def run(self):
while True:
time.sleep(1 / Geometry._frequency)
pos_lats = []
pos_lngs = []
Sensors.get_data(lambda obj: obj['name'] == 'latitude',
pos_lats, 3)
Sensors.get_data(lambda obj: obj['name'] == 'longitude',
pos_lngs, 3)
if len(pos_lats) < 3 or len(pos_lngs) < 3:
continue
p0, p1, p2 = ((a['value'], b['value'])
for a, b in zip(pos_lats, pos_lngs))
t0, t1, t2 = (pos_lats[i]['timestamp'] for i in range(3))
Geometry._time = t2
Geometry._pos = p2
Geometry._r = Geometry._make_r(p1, p2)
Geometry._v = Geometry._r / (t2 - t1)
Geometry._a = (Geometry._v - Geometry._make_r(p0, p1) /
(t1 - t0)) / ((t2 + t1) / 2 - (t1 + t0) / 2)
if self._last_stamp != t2:
if self._last_time is None:
self._last_time = time.time()
alph = 0.7
self.step_size = alph * self._step_size + \
(1 - alph) * (time.time() - self._last_time)
if Geometry._inter_pos is None:
Geometry._inter_pos = p2
# Stabilize
self._dir = 0.3 * (np.array(p2) - Geometry._inter_pos) + 0.7 * (
self._dir if self._dir is not None else np.array(p2) - Geometry._inter_pos)
self._last_stamp = t2
self._last_update_time = time.time()
self._last_time = time.time()
k = (time.time() - self._last_update_time) / self._step_size
self._last_update_time = time.time()
Geometry._inter_pos = list(self._dir * k + Geometry._inter_pos)
Geometry._build_marker()
def main():
json_data = open('config.json')
config = json.load(json_data)
print "Sensors"
sensors = Sensors(config)
readout = sensors.readall()
for read in readout:
print read
print " Temp: " + str(readout[read]['temp'])
print " Humidity: " + str(readout[read]['humidity'])
print "Devices"
devices = Devices(config)
readout = devices.readall()
for read in readout:
print read['name']
def run(self):
"""
Available data:
+ accelerator_pedal_position
- percentage
+ brake_pedal_status
- boolean
+ engine_speed
- 0 - 16382 (rpm)
+ torque_at_transmission
- Nm
+ vehicle_speed
- km/h
- fuel_consumed_since_restart
- fuel_level
- odometer
- steering_wheel_angle
- transmission_gear_position
"""
while True:
time.sleep(1.0 / self._frequency)
acc_ped_pos = []
vehicle_speed = []
Sensors.get_last(
lambda obj: obj['name'] == 'accelerator_pedal_position',
acc_ped_pos)
Sensors.get_last(
lambda obj: obj['name'] == 'vehicle_speed',
vehicle_speed)
acc_ped_pos = acc_ped_pos[0] if acc_ped_pos else None
vehicle_speed = vehicle_speed[0] if vehicle_speed else None
if acc_ped_pos is not None and vehicle_speed is not None:
timestamp = acc_ped_pos["timestamp"]
ped_pos = acc_ped_pos['value']
speed = vehicle_speed['value']
print("Accelerator pedal status {}".format(ped_pos))
print("Acceleration: {}".format(self.get_acceleration()))
if self.is_driving_aggressively(ped_pos):
print("AGGRESSIVE!")
self._event.set()
if self._start_time is None:
self._start_time = acc_ped_pos['timestamp']
self._send()
self._points.append((timestamp, ped_pos, speed))
def run(self):
while True:
time.sleep(1.0 / self._frequency)
wheel_rot = []
if Geometry._pos is None:
continue
lat, lng = Geometry._pos
Sensors.get_last(
lambda obj: obj['name'] == 'steering_wheel_angle',
wheel_rot,
max_age=1)
if wheel_rot:
wheel_rot = wheel_rot[0]['value']
if abs(wheel_rot) > 75 or self._street_address is None:
self._last_turn = time.time()
if self._last_turn is not None and time.time() - self._last_turn < 3:
address = self._street_address
try: # Haults (stucks) after quota of 2500 lookups per day.
response = self._cli.reverse_geocode(
(lat, lng), result_type="route")
response = response[0]
for line in response['address_components']:
if 'route' in line['types']:
address = line['long_name']
break
except Exception as e:
logging.debug(e)
if self._street_address is None:
self._street_address = address
elif address != self._street_address:
self._street_address = address
signaled = []
Sensors.get_data(
lambda e: e['name'] == 'turn_signals',
signaled,
max_age=12)
signaled = [e for e in signaled if e['value'] != 'off']
if not signaled:
self._event.set()
Distributor.analyzes.put({'name': 'forgot_signals', 'value': 1,
'timestamp': Geometry._time})
class Track():
def __init__(self):
self.sensors = Sensors()
self.displays = Displays()
self.gate = Gate()
def startRace(self):
self.displays.clear()
self.gate.release()
self.sensors.start()
def stopRace(self):
# No need to keep sensing when no race is happening
self.sensors.stop()
# Make sure we can reset the gate
self.gate.reset()
self.displays.displayTimes(self.sensors.getTimes())
return self.sensors.getTimes()
def getTimes(self):
return self.sensors.getTimes()
def test(self):
self.gate.release()
time.sleep(2)
self.gate.reset()
self.displays.displayHex([0xba5e, 0xba11, 0x0])
time.sleep(2)
self.displays.displayHex([0x0, 0xcafe, 0xbabe])
time.sleep(2)
self.displays.displayHex([0xdead, 0x0, 0xbeef])
time.sleep(2)
self.displays.clear()
time.sleep(1)
currentTime = time.time()
while (currentTime + 10.0) > time.time():
self.displays.displayTimes(self.sensors.getState())
self.displays.clear()
def run(self):
while True:
time.sleep(1.0 / self._frequency)
speed_lim, vehic_speed = [], []
Sensors.get_last(
lambda obj: obj['name'] == 'speed_limit',
speed_lim)
Sensors.get_last(
lambda obj: obj['name'] == 'vehicle_speed',
vehic_speed)
speed_lim = speed_lim[0]['value'] if speed_lim else None
vehic_speed = vehic_speed[0]['value'] if vehic_speed else None
if speed_lim:
pos = []
Sensors.get_last(lambda obj: obj['name'] == 'latitude', pos)
Sensors.get_last(lambda obj: obj['name'] == 'longitude', pos)
if vehic_speed and self.is_speeding(vehic_speed, speed_lim):
self._event.set()
self._event.speeding_percentage = (
vehic_speed - speed_lim) / speed_lim
# if self._velocities and self._event.speed_limit and speed_lim != self._event.speed_limit:
# self._send()
# if not self._start_time:
# self._start_time = pos[0]['timestamp']
# self._velocities.append(vehic_speed)
#self._points.append(list(map(lambda obj : obj['value'], pos)))
self._send()
# elif vehic_speed and self._start_time: # No longing speeding, has things to send
# self._end_time = pos[0]['timestamp']
# self._send()
self._event.speed_limit = speed_lim
def __init__(self):
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
GPIO.setwarnings(False)
# self.lock = threading.RLock()
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# Previous: [4, 17, 23, 24, 27, 22, 18, 5]
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
ledPins = [8, 16, 25, 12]
# CREATING FILE WITH PID
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# ----------------------
# TODO: change to self.Motor.Left
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
self.minDelayMotors = 2
self.state = enums.State.paused
self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16
self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6
self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8
self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13
self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7
self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10
self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11
self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x
self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x
self.lastTouch = [-1,-1,-1]
#print "light sensor value:"
#print self.sensors.readSensor(enums.SensorType.light, 1)
#if not isLightCalibrated:
#self.calibrateLight()
# try:
# thread.start_new_thread(self.pauseAndResume, ())
# except:
# print "Error: unable to start thread"
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
import threading
import time
import logging
from sensors import Sensors
import RPi.GPIO as GPIO
logging.basicConfig(level=logging.DEBUG,
format='(%(threadName)-10s) %(message)s',
)
# Global State
WC2_OCCUPIED = False
gpio = Sensors()
detection_counter = 0
usage_counter = 0
def wc2_worker():
global gpio
global usage_counter
# Check door
logging.debug(' #STEP 1 (check door) ------------')
start_first_check = time.time()
door_detection = GPIO.wait_for_edge(gpio.WC2_DOOR_sensor, GPIO.RISING, timeout=4500)
# check if door has been opened after 3 seconds
if door_detection is None:
logging.debug(' @Wc_2 door still closed, check first time duration: %.2f sec',round((time.time() - start_first_check), 2))
else:
'fg5' : '#0A1014',
'bg' : '#000000',
'bg1' : '#222222',
'red' : '#9E3A26',
'green' : '#80AA83',
}
separator = '^fg(' + col['bg1'] + ') ~ '
pico = 4
pbar = 5
d = dzen.DZen(dict(ico, **col))
timer = Timer()
net = NetStat('eth0')
cpu = CpuStat()
mem = MemStat()
sens = Sensors()
sens.update()
vol = AMixer('amixer get Master')
music = CMus()
# music = MPD("127.0.0.1", "6600")
i3wsbar = (
'/usr/bin/i3-wsbar',
'--show-all',
'-c', 'dzen2 -dock -expand left -y -1 -x %x -fn -*-DejaVu\ Sans\ Mono-normal-normal-normal-*-13-*-*-*-*-*-*-*',
'--input-on', 'DFP2',
'-output-on', 'DFP2'
)
pipe = os.pipe()
fd_r, fd_w = pipe
pid_child = os.fork()
class Tortoise:
"""
This is the class which implements the high-level behaviour of the tortoises. In order to make a proper use of the tortoises, an instance of this class should be created.
"""
def __init__(self):
"""
This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.
The reasons of termination of a user process could be because of normal termination
or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
and may be still on. In this case, the motors and LEDs should be turned off
for the battery not to drain.
The solution implemented is to have a background process (a watchdog) running
continously. This process checks if the user process doesn't exist anymore (termination).
If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
In order to identy that the user script has finished, a file with the name [PID].pid is
created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.
Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created.
However, this hasn't been implemented yet. The idea could be to save the light
values in a file and read that file when creating the tortoise object.
Light conditions could have changed, so this should be done carefully.
At the moment, the tortoise object is created without calibration. If the users
want to use the light sensors, they need will need to execute the calibrateLight
function before using those sensors.
"""
# Variables that control the calibration of the light sensors
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# --- Variables that control the calibration of the light sensors ---
# No warnings from the GPIO library
GPIO.setwarnings(False)
# Variables that control the random motion
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
# --- Variables that control the random motion ---
# Setting the motors, sensors and actuators
# Pin numbers of the motors
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
# Position 1 of the light sensors area in the PCB assigned to pin 17
self.sensors.setSensor(enums.SensorType.light, 1, 17)
# Position 2 of the light sensors area in the PCB assigned to pin 4
self.sensors.setSensor(enums.SensorType.light, 2, 4)
# Position 1 of the touch sensors area in the PCB assigned to pin 3
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3)
# Position 2 of the touch sensors area in the PCB assigned to pin 27
self.sensors.setSensor(enums.SensorType.touch, 2, 27)
# Position 3 of the touch sensors area in the PCB assigned to pin 2
self.sensors.setSensor(enums.SensorType.touch, 3, 2)
# Position 4 of the touch sensors area in the PCB assigned to pin 18
self.sensors.setSensor(enums.SensorType.touch, 4, 18)
# Position 1 of the proximity sensors area in the PCB assigned to pin 19
self.sensors.setSensor(enums.SensorType.proximity, 1, 19)
# Position 2 of the proximity sensors area in the PCB assigned to pin 21
self.sensors.setSensor(enums.SensorType.proximity, 2, 21)
# Position 3 of the proximity sensors area in the PCB assigned to pin 22
self.sensors.setSensor(enums.SensorType.proximity, 3, 22)
# Position 4 of the proximity sensors area in the PCB assigned to pin 26
self.sensors.setSensor(enums.SensorType.proximity, 4, 26)
# Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
ledPins = [8, 16, 25, 12]
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0])
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1])
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2])
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
# --- Setting the motors, sensors and actuators ---
# Times pressed the touch sensor for the latching behavour
self.lastTouch = [-1,-1,-1]
# Minimum milliseconds to send to the motors as delay
self.minDelayMotors = 2
# Creation of a file with the PID of the process
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# --- Creation of a file with the PID of the process ---
# Waiting for the user to press the e-stop button
self.state = enums.State.paused
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
# --- Waiting for the user to press the e-stop button ---
def getStateTortoise(self):
"""
Returns the state of the tortoise, either paused or running.
:rtype: enums.State
"""
return self.state
def setStateTortoise(self, toState):
self.state = toState
def calibrateLight(self):
"""
Calibrates the light sensor, defining the upper and lower bounds of the light, i.e. the values returned by the light sensor of the ambience and when a light source is placed in front of it.
Currently, only the light sensor in the position 1 of the PCB is used for calibration.
"""
global lowerBoundLight, upperBoundLight, isLightCalibrated
messages.printMessage('calibration_ambient')
raw_input()
lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
messages.printMessage('calibration_light_source')
raw_input()
upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
isLightCalibrated = True
messages.printMessage('calibration_complete')
def getSensorData(self, sensor_type, position):
"""
Returns a different value depending on the type of sensor queried:
- For the light sensor: an int in the range [0, 9]
- For the touch sensor: 1 if the sensor has been pressed since the last time it was queried, 0 if not
- For the e-stop: 1 if it's ON, 0 if it's OFF
- For the proximity sensor: 1 if there's an obstacle (it's ON), 0 if not (it's OFF)
:param sensor_type: type of the sensor queried
:param position: position in the PCB of the sensor queried
:type sensor_type: enums.SensorType
:type position: int
:rtype: int
"""
if (sensor_type == enums.SensorType.touch):
if (position < 1 or position > 3):
messages.printMessage('bad_touch_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.emergencyStop):
if (position != 4):
messages.printMessage('bad_emergency_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.light):
if (position != 1 and position!=2):
messages.printMessage('bad_light_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.proximity):
if (position < 1 or position > 4):
messages.printMessage('bad_proximity_sensor')
self.blinkLEDs_error()
return -1
else:
messages.printMessage('bad_sensor')
self.blinkLEDs_error()
return -1
# The value of the sensor is read at lowlevel
value = self.sensors.readSensor(sensor_type, position)
# For the light sensor, 'value' is the count of the light
if sensor_type == enums.SensorType.light:
return value
if (upperBoundLight - lowerBoundLight) == 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
# A scale to the range [0, 9] is done
lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))
if lightVal < 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
return lightVal
# For the touch sensor, 'value' is the number of times the sensor has been pressed
elif sensor_type == enums.SensorType.touch:
# Returns if the sensor has been pressed since the last time it was queried
return self.getSwitchTriggered(position,value)
# For the e-stop, 'value' is the number of times the sensor has been pressed
elif sensor_type == enums.SensorType.emergencyStop:
# Returns if it's either 1 (ON) or 0 (OFF)
return value % 2
# For the proximity sensor, 'value' is either 1 (ON) or 0 (OFF)
elif sensor_type == enums.SensorType.proximity:
# Returns 1 (ON) or 0 (OFF)
return value
def getSwitchTriggered(self, position, value):
if self.lastTouch[position-1] < 0:
self.lastTouch[position-1] = value
return 0
elif self.lastTouch[position-1] == value:
return 0
else:
self.lastTouch[position-1] = value
return 1
def getLEDValue(self, position):
"""
Returns 1 if the LED is ON, 0 if it's OFF.
:param position: position in the PCB of the sensor queried
:type position: int
:rtype: int
"""
if (position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
return self.actuators.getActuatorValue(enums.ActuatorType.led, position)
def setLEDValue(self, position, value):
"""
Turns on/off an LED.
:param position: position in the PCB of the sensor queried
:param value: 1 (ON) or 0 (OFF)
:type position: int
:type value: int
"""
if(position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
if(value != 0 and value != 1):
messages.printMessage('bad_LED_value')
self.blinkLEDs_error()
return -1
self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
return 0
def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):
"""
Blinks the LEDs wanted the number of times specified.
:param positions: position or positions in the PCB of the LEDs wanted
:param numberOfBlinks: number of blinks
:param delay: milliseconds to wait between blinking
:param blocking: whether the function should block forever or not.
:type positions: int, [int]
:type numberOfBlinks: int
:type delay: int
:type blocking: boolean
.. warning:: If blocking == True, the thread will block forever because of infinite loop. It's only used when there are errors.
"""
if numberOfBlinks < 0:
messages.printMessage('blinks_negative')
self.blinkLEDs_error()
return -1
if numberOfBlinks == 0:
messages.printMessage('blinks_zero')
self.blinkLEDs_error()
return -1
if delay < 0:
messages.printMessage('blinking_fast')
self.blinkLEDs_error()
return -1
# If no TypeError exception raised, 'positions' is an array
try:
# All positions are checked
for y in range(0, len(positions)):
if positions[y] < 0 or positions[y] > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
except TypeError: # It's not an array but an integer
if positions < 0 or positions > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
# The current state of the LEDs is saved to restore it later
previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]
cont = True
# If blocking == True, it's an infinite loop to "stop" the execution of the program and keep blinkind the LEDs
while cont:
for x in range(0, numberOfBlinks):
# If no TypeError exception raised, 'positions' is an array
try:
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)
time.sleep(delay)
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)
time.sleep(delay)
except TypeError: # It's not an array but an integer
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
time.sleep(delay)
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
time.sleep(delay)
# Depending on the parameter, it blocks or not
cont = blocking
# If it doesn't block, the previous state of the LEDs is restored
for x in range(1, 5):
self.setLEDValue(x, previousStateLEDs[x - 1])
return 0
def blinkLEDs_error(self):
self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True)
def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):
"""
Move the motors of the wheels.
Running the motors is done in different threads so that both wheels can move at the same time. The thread that executes this functions waits until the threads are finished, i.e. the motion is done. However, it doesn't block, but checks every half a second if the e-stop button has been pressed. If so, it stops the motors and exits.
:param stepsWheelA: number of rotations of the motor attached to wheel A
:param stepsWheelB: number of rotations of the motor attached to wheel B
:param delayWheelA: controls the speed of rotation of the motor attached to wheel A (minimum is 2 milliseconds)
:param delayWheelB: controls the speed of rotation of the motor attached to wheel B (minimum is 2 milliseconds)
:param direction: the direction in which the tortoise should move
:type stepsWheelA: int
:type stepsWheelB: int
:type delayWheelA: int
:type delayWheelB: int
:type direction: enums.Direction
"""
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_direction')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
messages.printMessage('bad_delay')
self.blinkLEDs_error()
return -1
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
else:
if self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
# The threads are created. They aren't started yet though.
motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))
# The specific wheels are started in order to accomplish the desired movement in the direction commanded
if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.clockwise:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.counterClockwise:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
# The main loop pools the emergencyStop while the motors are running
while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():
# If a stop command has been sent, the turtle will stop its movement and exit this function
if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
if motorAprocess_backwards.is_alive():
motorAprocess_backwards.terminate()
motorAprocess_backwards.join()
if motorBprocess_backwards.is_alive():
motorBprocess_backwards.terminate()
motorBprocess_backwards.join()
if motorAprocess_forwards.is_alive():
motorAprocess_forwards.terminate()
motorAprocess_forwards.join()
if motorBprocess_forwards.is_alive():
motorBprocess_forwards.terminate()
motorBprocess_forwards.join()
elif self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
time.sleep(0.5)
# When the movement finishes, the motors are turned off
self.A.stopMotors()
self.B.stopMotors()
return 0
def moveForwards(self, steps):
"""
The tortoise moves forwards.
:param steps: number of rotations of the motors
:type steps: int
"""
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards)
def moveBackwards(self, steps):
"""
The tortoise moves backwards.
:param steps: number of rotations of the motors
:type steps: int
"""
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards)
def turnOnTheSpot(self, steps, direction):
"""
This function makes the tortoise turn with the centre of rotation in one of the wheels.
:param steps: number of rotations of the motors
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type steps: int
:type direction: enums.Direction
"""
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
# Only wheel A moves
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)
# Only wheel B moves
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)
def shuffleOnTheSpot(self, steps, direction):
"""
This function makes the tortoise turn with the centre of rotation between both wheels.
:param steps: number of rotations of the motors
:param direction: either clockwise or counter-clockwise
:type steps: int
:type direction: enums.Direction
"""
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_shuffle')
self.blinkLEDs_error()
return -1
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)
def shuffle45degrees(self, direction):
"""
This function tries to make the tortoise shuffle 45 degrees.
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type direction: enums.Direction
"""
return self.shuffleOnTheSpot(180, direction)
def turn(self, stepsWheelA, stepsWheelB, direction):
"""
This function makes the tortoise turn by specifying different steps for the wheels.
The function computes the delay that the wheel with less rotations should have in order to finish at the same time than the other wheel.
:param stepsWheelA: number of rotations of the motor attached to wheel A
:param stepsWheelB: number of rotations of the motor attached to wheel B
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type stepsWheelA: int
:type stepsWheelB: int
:type direction: enums.Direction
"""
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
# The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB
return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
# The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA
return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)
def turn45degrees_sharp(self, direction):
"""
This function tries to make the tortoise turn 45 degrees sharply.
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type direction: enums.Direction
"""
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.turn(400, 75, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.turn(75, 400, direction)
def turn30degrees_wide(self, direction):
"""
This function tries to make the tortoise turn 45 degrees wide.
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type direction: enums.Direction
"""
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.turn(450, 250, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.turn(250, 450, direction)
def doRandomMovement(self):
"""
Performs a natural random movement.
The function chooses a movement, and it can be repeated up to three times. The probabilities of repeating the movement are as follows:
- No repetition: 60%
- Once: 25%
- Twice: 10%
- Three times: 5%
The probabilities of choosing a random movement are as follows:
- Move forwards: 30%
- Move backwards: 10%
- Shuffling: 10%
- Turning 30 or 45 degrees: 10%
- Turning with random steps: 40%
The direction of movement for the turns and shuffles is chosen randomly.
"""
# New random command
if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand:
# The number of times the command is repeated is chosen randomly with decreasing probabilities up to 3 times.
self.numberRepeatsRandomCommand = np.random.choice([0, 1, 2, 3], 1, p = [0.6, 0.25, 0.1, 0.05])
# As this is a new command, no repetitions done
self.timesSameRandomCommandExecuted = 0
# Random steps for wheel A
self.lastRandomStepsWheelA = np.random.randint(30, 300)
# Random number between 0 and 1 for decision on the random movement
randomNumber = np.random.random_sample()
# 40% probability of moving forwards/backwards
if(randomNumber < 0.4):
# 75% of probability of moving forwards
if(randomNumber < 0.30):
self.moveForwards(self.lastRandomStepsWheelA)
self.lastRandomCommand = self.moveForwards
# 25% probability of moving backwards
else:
self.moveBackwards(self.lastRandomStepsWheelA)
self.lastRandomCommand = self.moveBackwards
# 10% probability of shuffling
elif (randomNumber < 0.5):
# Equal probability of going clockwise or counter clockwise
self.lastRandomDirection = np.random.choice([enums.Direction.clockwise, enums.Direction.counterClockwise], 1)
self.shuffle45degrees(self.lastRandomDirection)
self.lastRandomCommand = self.shuffle45degrees
# 10% probability of turning 30 or 45 degrees
elif (randomNumber < 0.6):
# Equal probability of moving forwards/backwards lef/right
self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1)
# Equal probability of turning 30 degrees wide or 45 degrees sharp
if(randomNumber < 0.55):
self.turn45degrees_sharp(self.lastRandomDirection)
self.lastRandomCommand = self.turn45degrees_sharp
else:
self.turn30degrees_wide(self.lastRandomDirection)
self.lastRandomCommand = self.turn30degrees_wide
# 40% of turning randomly
else:
# Random steps for wheel B
self.lastRandomStepsWheelB = np.random.randint(30, 300)
# Equal probability of moving forwards/backwards lef/right
self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1)
if(self.lastRandomDirection == enums.Direction.forwards_left or self.lastRandomDirection == enums.Direction.backwards_left):
if(self.lastRandomStepsWheelA >= self.lastRandomStepsWheelB):
aux = self.lastRandomStepsWheelA
self.lastRandomStepsWheelA = self.lastRandomStepsWheelB - 1 # To avoid the case of equals
self.lastRandomStepsWheelB = aux
else:
if(self.lastRandomStepsWheelB >= self.lastRandomStepsWheelA):
aux = self.lastRandomStepsWheelA
self.lastRandomStepsWheelA = self.lastRandomStepsWheelB
self.lastRandomStepsWheelB = aux - 1 # To avoid the case of equals
self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection)
self.lastRandomCommand = self.turn
# Repeat last command
else:
self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1
if self.lastRandomCommand == self.moveForwards:
self.moveForwards(self.lastRandomStepsWheelA)
elif self.lastRandomCommand == self.moveBackwards:
self.moveBackwards(self.lastRandomStepsWheelA)
elif self.lastRandomCommand == self.shuffle45degrees:
self.shuffle45degrees(self.lastRandomDirection)
elif self.lastRandomCommand == self.turn45degrees_sharp:
self.turn45degrees_sharp(self.lastRandomDirection)
elif self.lastRandomCommand == self.turn30degrees_wide:
self.turn30degrees_wide(self.lastRandomDirection)
elif self.lastRandomCommand == self.turn:
self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection)
def __init__(self):
self.sensors = Sensors()
self.displays = Displays()
self.gate = Gate()
from sensors import Sensors import RPi.GPIO as GPIO gpio = Sensors() GPIO.wait_for_edge(gpio.WC2_DOOR_sensor, GPIO.FALLING) door_close_time= time.time() while gpio.is_wc2_motion_detected_by_PIR(): pass move_detection_stop = time.time() print "Time: %.2f " % (move_detection_stop - door_close_time)
class ObstacleAvoidance(StoppableThread):
# Class constants
LEFT = "left"
RIGHT = "right"
UP = "up"
DEVIATION_HORIZONTAL = 20
DEVIATION_VERTICAL = 500
NO_OBSTACLES_AHEAD = 1
CAUTION_DISTANCE = 200 # Should be 4000, Measured in Centimeters
DANGER_DISTANCE = 125 # Should be 1000, Measured in Centimeters
CAUTION_ALTITUDE = 375 # Should be 3000, Measured in Centimeters.
DANGER_ALTITUDE = 4000 # Should be 4000, Measured in Centimeters.
CONSTANT_HEIGHT = 1000 # Should be 1000, Measured in Centimeters.
MAX_LEFT_RIGHT = 0.875 # Should be 7.0, Maximum distance the drone would go right/left until trying to pass and obstacle from above. Measured in Meters.
LEFT_SENSOR = "leftSensor"
RIGHT_SENSOR = "rightSensor"
FRONT_SENSOR = "frontSensor"
BOTTOM_SENSOR = "bottomSensor"
# In the final software, the __init__ function signature should look like:
# def __init__(self, vehicle):
# vehicle object is an object from external module that has been developed by a different person within
# the company. Since this module use other modules which is not related
# to this project, I put all the relevant code line for proper functioning in comment.
# def __init__(self, vehicle):
def __init__(self):
# # Check for correct input
# if isinstance(vehicle, Vehicle) is False:
# raise TypeError('Expected object of type Vehicle, got '+type(vehicle).__name__)
# Calling super class constructor
StoppableThread.__init__(self)
# These are the distances the drone passed in a certain direction.
# They are used to know if I need to try to pass the obstacle from another direction.
self.__right_distance = 0.0
self.__left_distance = 0.0
self.__up_distance = 0.0
# Always keep track of my last move in order to know better what to do in certain situations.
self.__last_move = None
# In case the drone need to pass an obstacle from above - keeping 2 flags. One for indicating its in the
# process of climbing, and another one to indicate it finished climbing and should now keep its altitude
# until passing the obstacle.
self.__keep_altitude = False
self.__climbing = False
# I want to maintain the drone's altitude for a short period of time before descending back to the
# original constant altitude, so for that I keep tracking of the time since it ascended.
self.__start_time_measure = 0.0
# In case of emergency, keeping a flag to order the drone to land.
self.__safety_protocol = False
# Other classes within the software for giving flight orders, get sensors data and
# calculate distances by geographic positioning system data.
# the __flight_commands & __location objects should get as a parameter the vehicle object
self.__flight_commands = FlightCommands() # FlightCommands(vehicle)
self.__sensors = Sensors()
self.__flight_data = FlightData() # FlightData(vehicle)
# Get the drone's location and height for further calculations.
self.__last_latitude = self.__flight_data.get_current_latitude()
self.__last_longitude = self.__flight_data.get_current_longitude()
self.__last_height = self.__flight_data.get_current_height()
# Counter and flag for being aware of a sequence of illegal sensor inputs in order to be aware of a
# malfunction in the system
self.__illegal_input_counter = 0
self.__last_input_legitimacy = True
self.__num_of_lines = int(self.__get_number_of_line_in_file()) # Delete when moving to a full functioning system.
# Initializing the sensors
if self.__sensors.connect() == -1:
raise ConnectionError('Cannot connect to sensors')
print("Connected Successfuly to Sensors!")
# Flag that indicates if the system is activated in order to give the user the knowledge if it should override
# other flight commands given to the drone. The flag is 'True' only for left/right/up maneuvers and False
# for all other cases including fixing the drone's altitude, since the altitude is fixed for 10 meters and
# any other running software within the drone shouldn't change its height.
self.__is_active = False
def run(self):
while not self.stopped():
simulator.altitude_change() # Delete when moving to a full functioning system.
self.__num_of_lines -= 1 # Delete when moving to a full functioning system.
if self.__num_of_lines == 0: # Delete when moving to a full functioning system.
self.stopit() # Delete when moving to a full functioning system.
continue # Delete when moving to a full functioning system.
print("-----------------------------------------------------------")
if self.__safety_protocol is True:
self.__follow_safety_protocol()
ahead_distance = self.__get_sensor_reading(self.FRONT_SENSOR)
print("Distance Ahead: %d" % ahead_distance)
# In case the path ahead is clear of obstacles, reset counters and fix altitude.
if ahead_distance == self.NO_OBSTACLES_AHEAD or ahead_distance >= self.CAUTION_DISTANCE:
print("Way is Clear")
self.__check_flags()
self.__fix_altitude()
self.__last_move = None
self.__is_active = False
print("Is Active = " + str(self.__is_active))
self.__right_distance = self.__left_distance = self.__up_distance = 0.0
continue
if ahead_distance <= self.DANGER_DISTANCE:
# There's an obstacle in less than 10 meters - DANGER!
# In such case the algorithm would slow down the drone drastically in order to give it more
# time to manouver and avoid a collision.
print("CAUTION: Obstacle in less than 1.25 meters!")
print("Slowing Down to avoid collision")
self.__flight_commands.slow_down(ahead_distance)
else:
print("Obstacle in less than 2 meters")
self.__is_active = True
print("Is Active = " + str(self.__is_active))
# Get a reading from the left side sensor.
left_side_distance = self.__get_sensor_reading(self.LEFT_SENSOR)
# Get a reading from the right side sensor.
right_side_distance = self.__get_sensor_reading(self.RIGHT_SENSOR)
print("Distance Left: %d, Distance Right: %d" % (left_side_distance, right_side_distance))
# If already tried going to go to the left/right 7 meters and there's
# still an obstacle ahead then I want to try from above.
if self.__need_to_go_up() is True:
self.__climbing = True
if self.__flight_data.get_current_height() >= self.DANGER_ALTITUDE:
self.__safety_protocol = True
self.__follow_safety_protocol()
else:
self.__move_in_direction(self.UP)
print("Going up")
continue
# Check if right side is clear.
elif right_side_distance > left_side_distance + self.DEVIATION_HORIZONTAL:
self.__move_in_direction(self.RIGHT)
self.__check_flags()
print("Going right")
# Check if left side is clear.
elif left_side_distance > right_side_distance + self.DEVIATION_HORIZONTAL:
self.__move_in_direction(self.LEFT)
self.__check_flags()
print("Going left")
# If both left and right gives about the same distance.
elif self.__climbing:
if self.__flight_data.get_current_height() >= self.DANGER_ALTITUDE:
self.__safety_protocol = True
self.__follow_safety_protocol()
continue
else:
self.__move_in_direction(self.UP)
print("Going up")
continue
# If both left and right side looks blocked and still want to try to pass the obstacle from one of its sides
else:
if self.__last_move is not None:
self.__move_in_direction(self.__last_move)
print("Going " + self.__last_move)
else:
if left_side_distance > right_side_distance:
self.__move_in_direction(self.LEFT)
print("Going left")
elif left_side_distance < right_side_distance:
self.__move_in_direction(self.RIGHT)
print("Going right")
self.__fix_altitude()
def __need_to_go_up(self):
if self.__right_distance >= self.MAX_LEFT_RIGHT or self.__left_distance >= self.MAX_LEFT_RIGHT:
return True
else:
return False
def __move_in_direction(self, direction):
if direction == self.RIGHT:
self.__flight_commands.go_right()
elif direction == self.LEFT:
self.__flight_commands.go_left()
elif direction == self.UP:
self.__flight_commands.go_up()
self.__keep_altitude = True
self.__start_time_measure = round(time.time())
elif type(direction).__name__ is "str":
raise ValueError('Expected "' + self.UP + '" / "' + self.LEFT + '" / "' + self.RIGHT + '", instead got ' +
str(direction))
else:
raise TypeError('Expected variable of type str and got a variable of type ' + type(direction).__name__)
self.__update_distance(direction)
self.__last_move = direction
def __update_distance(self, direction):
if direction != self.RIGHT \
and direction != self.LEFT \
and direction != self.UP \
and isinstance(direction, str):
raise ValueError('Expected "' + self.UP + '" / "' + self.LEFT + '" / "' + self.RIGHT + '", instead got ' +
str(direction))
elif type(direction).__name__ != "str":
raise TypeError('Expected variable of type str and got a variable of type ' + type(direction).__name__)
# Get current location data.
current_latitude = self.__flight_data.get_current_latitude()
current_longitude = self.__flight_data.get_current_longitude()
current_height = self.__flight_data.get_current_height()
delta = self.__flight_data.calculate_distance(
self.__last_latitude, self.__last_longitude, current_latitude, current_longitude)
# Update the distance travelled in certain direction
if direction == self.RIGHT:
self.__right_distance += delta
self.__left_distance = 0
print("Distance went right: %f" % self.__right_distance)
elif direction == self.LEFT:
self.__left_distance += delta
self.__right_distance = 0
print("Distance went left: %f" % self.__left_distance)
elif direction == self.UP:
self.__up_distance += current_height - self.__last_height
self.__left_distance = self.__right_distance = 0
print("Distance went up: %f" % self.__up_distance)
# Update last known location attributes
self.__last_latitude = current_latitude
self.__last_longitude = current_longitude
self.__last_height = current_height
def __get_sensor_reading(self, sensor):
legal_input = False
while legal_input is False:
if sensor is self.FRONT_SENSOR:
distance = self.__sensors.check_ahead()
elif sensor is self.RIGHT_SENSOR:
distance = self.__sensors.check_right_side()
elif sensor is self.LEFT_SENSOR:
distance = self.__sensors.check_left_side()
elif sensor is self.BOTTOM_SENSOR:
distance = self.__sensors.check_below()
else:
if isinstance(sensor, str):
raise ValueError('Expected "' + self.FRONT_SENSOR + '" / "' + self.BOTTOM_SENSOR + '" / "' +
self.LEFT_SENSOR + '" / "' + self.RIGHT_SENSOR + '", instead got ' + sensor)
else:
raise TypeError('Expected variable of type str and got a variable of type ' + type(sensor).__name__)
legal_input = self.__check_measurement(distance)
if legal_input:
return distance
def __check_measurement(self, measurement):
is_int = isinstance(measurement, int)
if is_int is False and measurement is not "NACK":
raise TypeError('Expected variable of type int and got a variable of type ' + type(measurement).__name__)
if is_int:
if measurement > 0:
self.__last_input_legitimacy = True
return True
if self.__last_input_legitimacy is True:
self.__illegal_input_counter = 1
else:
self.__illegal_input_counter += 1
if self.__illegal_input_counter >= 10:
raise SystemError('Malfunction in sensors, check physical connections')
self.__last_input_legitimacy = False
return False
def __check_flags(self):
if self.__climbing is True:
self.__climbing = False
self.__keep_altitude = True
def __safe_for_landing(self):
print("Check if safe to land")
drone_altitude = self.__flight_data.get_current_height()
bottom_sensor_distance = self.__get_sensor_reading(self.BOTTOM_SENSOR)
print("Pixhawk height: " + str(drone_altitude) + ", Sensor height: " + str(bottom_sensor_distance))
heigh_difference = math.fabs(bottom_sensor_distance - drone_altitude)
if heigh_difference > self.DEVIATION_HORIZONTAL:
return False
else:
return True
def __fix_altitude(self):
bottom_sensor_distance = self.__get_sensor_reading(self.BOTTOM_SENSOR)
altitude = self.__flight_data.get_current_height()
print("Sensor Below: " + str(bottom_sensor_distance) + ", Pixhawk: " + str(altitude))
if bottom_sensor_distance > self.DANGER_ALTITUDE:
self.__safety_protocol = True
self.__follow_safety_protocol()
return
if self.__keep_altitude:
# This means the drone passed an obstacle from above. in that case I want to maintain my current altitude
# for 10 seconds to make sure the drone passed the obstacle, before getting back to regular altitude.
current_time = int(round(time.time()))
print("maintaining altitude for " + str((current_time - self.__start_time_measure)) + " seconds")
if current_time - self.__start_time_measure > 10.0:
# 10 Seconds have passed, now before the drone start decending I want to make sure
# there's nothing below and its safe for him to descend.
self.__keep_altitude = False
else:
self.__flight_commands.maintain_altitude()
return
# Fix the height of the drone to 10 meters from the ground.
delta_altitude = self.CONSTANT_HEIGHT - bottom_sensor_distance
if math.fabs(delta_altitude) < self.DEVIATION_HORIZONTAL:
return
elif delta_altitude > 0:
self.__flight_commands.go_up()
print("Fixing altitude - Going up")
elif delta_altitude < 0:
self.__flight_commands.go_down()
print("Fixing altitude - Going down")
def __get_number_of_line_in_file(self):
i = 0
with open('Sensors Data\\leftSensorData.txt') as file:
for i, l in enumerate(file):
pass
return i + 1
def __follow_safety_protocol(self):
# If the code reached here it means the drone raised 25 meters up and still see an obstacle.
# so in this case we prefer it would abort the mission in so it won't get too high or damaged.
if self.__safe_for_landing():
# while(self.__get_sensor_reading(self.__BOTTOM_SENSOR) > 10):
self.__flight_commands.land() # Enter this into the while loop above.
self.stopit()
else:
self.__flight_commands.go_back_to_base()
def take_control(self):
return self.__is_active
def __init__(self):
"""
This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.
The reasons of termination of a user process could be because of normal termination
or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
and may be still on. In this case, the motors and LEDs should be turned off
for the battery not to drain.
The solution implemented is to have a background process (a watchdog) running
continously. This process checks if the user process doesn't exist anymore (termination).
If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
In order to identy that the user script has finished, a file with the name [PID].pid is
created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.
Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created.
However, this hasn't been implemented yet. The idea could be to save the light
values in a file and read that file when creating the tortoise object.
Light conditions could have changed, so this should be done carefully.
At the moment, the tortoise object is created without calibration. If the users
want to use the light sensors, they need will need to execute the calibrateLight
function before using those sensors.
"""
# Variables that control the calibration of the light sensors
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# --- Variables that control the calibration of the light sensors ---
# No warnings from the GPIO library
GPIO.setwarnings(False)
# Variables that control the random motion
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
# --- Variables that control the random motion ---
# Setting the motors, sensors and actuators
# Pin numbers of the motors
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
# Position 1 of the light sensors area in the PCB assigned to pin 17
self.sensors.setSensor(enums.SensorType.light, 1, 17)
# Position 2 of the light sensors area in the PCB assigned to pin 4
self.sensors.setSensor(enums.SensorType.light, 2, 4)
# Position 1 of the touch sensors area in the PCB assigned to pin 3
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3)
# Position 2 of the touch sensors area in the PCB assigned to pin 27
self.sensors.setSensor(enums.SensorType.touch, 2, 27)
# Position 3 of the touch sensors area in the PCB assigned to pin 2
self.sensors.setSensor(enums.SensorType.touch, 3, 2)
# Position 4 of the touch sensors area in the PCB assigned to pin 18
self.sensors.setSensor(enums.SensorType.touch, 4, 18)
# Position 1 of the proximity sensors area in the PCB assigned to pin 19
self.sensors.setSensor(enums.SensorType.proximity, 1, 19)
# Position 2 of the proximity sensors area in the PCB assigned to pin 21
self.sensors.setSensor(enums.SensorType.proximity, 2, 21)
# Position 3 of the proximity sensors area in the PCB assigned to pin 22
self.sensors.setSensor(enums.SensorType.proximity, 3, 22)
# Position 4 of the proximity sensors area in the PCB assigned to pin 26
self.sensors.setSensor(enums.SensorType.proximity, 4, 26)
# Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
ledPins = [8, 16, 25, 12]
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0])
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1])
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2])
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
# --- Setting the motors, sensors and actuators ---
# Times pressed the touch sensor for the latching behavour
self.lastTouch = [-1,-1,-1]
# Minimum milliseconds to send to the motors as delay
self.minDelayMotors = 2
# Creation of a file with the PID of the process
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# --- Creation of a file with the PID of the process ---
# Waiting for the user to press the e-stop button
self.state = enums.State.paused
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
class Main():
def __init__(self):
self.filename = "config.txt"
with open(self.filename) as f:
config = f.read()
self.config_list = [y for y in (x.strip() for x in config.splitlines()) if y]
if self.find("onetouch") == "True":
self.onetouch = True
else:
self.onetouch = False
self.mysens = Sensors()
self.mouse = Plattform()
self.recived_data = None
self.count = [0, 0, 0, 0,0]
self.controlboard = []
self.oldtouch = [False, False,False, False,False]
self.touch = [False, False,False, False,False]
self.sensibility = 0
self.actived = True
self.oldx = None
self.oldy = None
self.AS = [None,None,None] #X,Y,Z
self.AA = [None, None, None] # X,Y,Z
self.GS = [None, None, None] # X,Y,Z
self.GA = [None, None, None] # X,Y,Z
self.mysens.setGyroscopeAngolData(X = 0.0, Y = 0.0, Z = 0.0)
self.mysens.setGyroscopeScalledData(X=0.0, Y=0.0, Z=0.0)
def find(self, keyword):
return [s for s in self.config_list if keyword in s][0].split(" ")[1]
def controlGyroscope(self):
data = self.mysens.getGyroscopeScalledData()
data2 = self.mysens.getGyroscopeAngolData()
#print(data)
if "SG" in self.recived_data:
if self.recived_data[2] == "X":
data[0] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Y":
data[1] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Z":
data[2] = float(self.recived_data.split(" ")[1])
if "AG" in self.recived_data:
if self.recived_data[2] == "X":
data2[0] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Y":
data2[1] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Z":
data2[2] = float(self.recived_data.split(" ")[1])
self.mysens.setGyroscopeScalledData(X = data[0], Y = data[1], Z = data[2])
self.mysens.setGyroscopeAngolData(X=data2[0], Y=data2[1], Z=data2[2])
def controlAccelerometer(self):
#accelerometer data
data = self.mysens.getAccelerometerScalledData()
data2 = self.mysens.getAccelerometerScalledData()
if "SA" in self.recived_data:
if self.recived_data[2] == "X":
data[0] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Y":
data[1] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Z":
data[2] = float(self.recived_data.split(" ")[1])
if "AA" in self.recived_data:
if self.recived_data[2] == "X":
data2[0] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Y":
data2[1] = float(self.recived_data.split(" ")[1])
if self.recived_data[2] == "Z":
data2[2] = float(self.recived_data.split(" ")[1])
self.mysens.setAccelerometerScalledData(X=data[0], Y=data[1], Z=data[2])
self.mysens.setAccelerometerAngolData(X=data2[0], Y=data2[1], Z=data2[2])
def controlOtherSensor(self):
if self.recived_data.split(" ")[0] == "Temperature:": self.mysens.setTemperature(self.recived_data.split(" ")[1])
#touch sensor data
first = self.mysens.getTouchSensorFromIndex(0)
second = self.mysens.getTouchSensorFromIndex(1)
third = self.mysens.getTouchSensorFromIndex(2)
fourth = self.mysens.getTouchSensorFromIndex(3)
fifth = self.mysens.getTouchSensorFromIndex(4)
if self.recived_data.split(" ")[0] == "Sensor1:":
if int(self.recived_data.split(" ")[1]) == 1:
first = True
if int(self.recived_data.split(" ")[1]) == 0:
first = False
if self.recived_data.split(" ")[0] == "Sensor2:":
if int(self.recived_data.split(" ")[1]) == 1:
second = True
if int(self.recived_data.split(" ")[1]) == 0:
second = False
if self.recived_data.split(" ")[0] == "Sensor3:":
if int(self.recived_data.split(" ")[1]) == 1:
third = True
if int(self.recived_data.split(" ")[1]) == 0:
third = False
if self.recived_data.split(" ")[0] == "Sensor4:":
if int(self.recived_data.split(" ")[1]) == 1:
fourth = True
if int(self.recived_data.split(" ")[1]) == 0:
fourth = False
if self.recived_data.split(" ")[0] == "Sensor5:":
if int(self.recived_data.split(" ")[1]) == 1:
fifth = True
if int(self.recived_data.split(" ")[1]) == 0:
fifth = False
self.mysens.setTouchSensor(FIRST_SENSOR=first, SECOND_SENSOR=second,
THIRD_SENSOR=third,FOURTH_SENSOR= fourth, FIFTH_SENSOR=fifth)
def setRecivedData(self, data):
self.recived_data = data
def controlTouch(self):
self.touch = self.mysens.getTouchSensors()
if self.touch[0] == self.oldtouch[0]:
pass
else:
self.oldtouch[0] = self.touch[0]
if self.count[0] == 1:
self.count[0] = 0
elif self.count[0] == 0:
self.count[0] = 1
print("Sensor 1 pressed")
self.pressKey(self.find("p1"))
if self.touch[1] == self.oldtouch[1]:
pass
else:
self.oldtouch[1] = self.touch[1]
if self.count[1] == 1:
self.count[1] = 0
elif self.count[1] == 0:
self.count[1] = 1
print("Sensor 2 pressed")
self.pressKey(self.find("p2"))
if self.touch[2] == self.oldtouch[2]:
pass
else:
self.oldtouch[2] = self.touch[2]
if self.count[2] == 1:
self.count[2] = 0
elif self.count[2] == 0:
self.count[2] = 1
print("Sensor 3 pressed")
self.pressKey(self.find("p3"))
if self.touch[3] == self.oldtouch[3]:
pass
else:
self.oldtouch[3] = self.touch[3]
if self.count[3] == 1:
self.count[3] = 0
print("Sensor 4 pressed")
self.pressKey(self.find("p4"))
elif self.count[3] == 0:
self.count[3] = 1
if self.touch[4] == self.oldtouch[4]:
pass
else:
self.oldtouch[4] = self.touch[4]
if self.count[4] == 1:
self.count[4] = 0
elif self.count[4] == 0:
self.count[4] = 1
print("Sensor 5 pressed")
self.pressKey(self.find("p5"))
def setSensibility(self, sens):
self.sensibility = sens
def moveMouseFromGlove(self):
#here i trasform gyroscope data in angles
pass
# if angles are > i move mouse in one direction, else, in the other direction
data = self.mysens.getGyroscopeScalledData()
x = int(data[2]*self.sensibility) #for the gyroscope this is the z
y = int(data[0]*self.sensibility) #for the gyroscope this is the x
x1,y1 = self.mouse.getCursor()
self.mouse.moveCursor(x + x1, y + y1)
def moveMouseFromBand(self):
# here i trasform gyroscope data in angles
pass
# if angles are > i move mouse in one direction, else, in the other direction
data = self.mysens.getGyroscopeScalledData()
print(data)
x = int(data[0] * self.sensibility) # for the gyroscope this is the z
y = int(data[1] * self.sensibility) # for the gyroscope this is the x
x1, y1 = self.mouse.getCursor()
self.mouse.moveCursor(x + x1, y + y1)
def pressKey(self,key):
if key == "Active":
if self.actived == True:
self.actived = False
elif self.actived == False:
self.actived = True
elif key == "False":
pass
else:
try:
SendKeys.SendKeys(key)
except:
print("Key doesn't recognized")
def getActived(self):
return self.actived
class Tortoise:
def __init__(self):
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
GPIO.setwarnings(False)
# self.lock = threading.RLock()
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# Previous: [4, 17, 23, 24, 27, 22, 18, 5]
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
ledPins = [8, 16, 25, 12]
# CREATING FILE WITH PID
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# ----------------------
# TODO: change to self.Motor.Left
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
self.minDelayMotors = 2
self.state = enums.State.paused
self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16
self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6
self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8
self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13
self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7
self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10
self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11
self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x
self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x
self.lastTouch = [-1,-1,-1]
#print "light sensor value:"
#print self.sensors.readSensor(enums.SensorType.light, 1)
#if not isLightCalibrated:
#self.calibrateLight()
# try:
# thread.start_new_thread(self.pauseAndResume, ())
# except:
# print "Error: unable to start thread"
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
def getStateTortoise(self):
return self.state
def setStateTortoise(self, toState):
self.state = toState
def calibrateLight(self):
global lowerBoundLight, upperBoundLight, isLightCalibrated
messages.printMessage('calibration_ambient')
raw_input()
#lowerBoundLight = max(self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2))
lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
#print "Light in normal conditions is: ", lowerBoundLight
messages.printMessage('calibration_light_source')
raw_input()
#upperBoundLight = min((self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2)))
upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
# print "Light when there is a light source is:", upperBoundLight
isLightCalibrated = True
messages.printMessage('calibration_complete')
def getSensorData(self, sensor_type, position):
if (sensor_type == enums.SensorType.touch):
if (position < 1 or position > 3):
messages.printMessage('bad_touch_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.light):
if (position != 1 and position!=2):
messages.printMessage('bad_light_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.proximity):
if (position < 1 or position > 4):
messages.printMessage('bad_proximity_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.emergencyStop):
if (position != 1):
messages.printMessage('bad_emergency_sensor')
self.blinkLEDs_error()
return -1
else:
messages.printMessage('bad_sensor')
self.blinkLEDs_error()
return -1
value = self.sensors.readSensor(sensor_type, position)
if sensor_type == enums.SensorType.light:
return value
if (upperBoundLight - lowerBoundLight) == 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
# Scale
lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))
if lightVal < 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
return lightVal
elif sensor_type == enums.SensorType.touch:
return self.getSwitchTriggered(position,value)
elif sensor_type == enums.SensorType.emergencyStop:
return value % 2
else:
return value
def getSwitchTriggered(self, position, value):
if self.lastTouch[position-1]<0:
self.lastTouch[position-1]=value
return 0
elif self.lastTouch[position-1]==value:
return 0
else:
self.lastTouch[position-1]=value
return 1
def getLEDValue(self, position):
if (position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
return self.actuators.getActuatorValue(enums.ActuatorType.led, position)
def setLEDValue(self, position, value):
if(position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
if(value != 0 and value != 1):
messages.printMessage('bad_LED_value')
self.blinkLEDs_error()
return -1
self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
return 0
def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):
if numberOfBlinks < 0:
messages.printMessage('blinks_negative')
self.blinkLEDs_error()
return -1
if numberOfBlinks == 0:
messages.printMessage('blinks_zero')
self.blinkLEDs_error()
return -1
if delay < 0:
messages.printMessage('blinking_fast')
self.blinkLEDs_error()
return -1
try:
for y in range(0, len(positions)):
if positions[y] < 0 or positions[y] > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
except TypeError: # It's not an array but an integer
if positions < 0 or positions > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]
cont = True
# Infinite loop to "stop" the execution of the program and keep blinkind the LEDs
while cont:
for x in range(0, numberOfBlinks):
try:
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)
time.sleep(delay)
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)
time.sleep(delay)
except TypeError: # It's not an array but an integer
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
time.sleep(delay)
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
time.sleep(delay)
cont = blocking
# If it doesn't block, the previous state of the LEDs is restored
for x in range(1, 5):
self.setLEDValue(x, previousStateLEDs[x - 1])
return 0
def blinkLEDs_error(self):
self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True)
def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_direction')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
messages.printMessage('bad_delay')
self.blinkLEDs_error()
return -1
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
else:
if self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))
if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.clockwise:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.counterClockwise:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
# The main loop pools the emergencyStop
while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
if motorAprocess_backwards.is_alive():
motorAprocess_backwards.terminate()
motorAprocess_backwards.join()
if motorBprocess_backwards.is_alive():
motorBprocess_backwards.terminate()
motorBprocess_backwards.join()
if motorAprocess_forwards.is_alive():
motorAprocess_forwards.terminate()
motorAprocess_forwards.join()
if motorBprocess_forwards.is_alive():
motorBprocess_forwards.terminate()
motorBprocess_forwards.join()
elif self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
time.sleep(0.5)
self.A.stopMotors()
self.B.stopMotors()
return 0
def moveForwards(self, steps):
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards)
def moveBackwards(self, steps):
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards)
def turnOnTheSpot(self, steps, direction):
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)
def shuffleOnTheSpot(self, steps, direction):
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_shuffle')
self.blinkLEDs_error()
return -1
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)
def turn(self, stepsWheelA, stepsWheelB, direction):
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB
return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA
return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)
def doRandomMovement2(self):
maxTimesCommandRepeated = 3
# New random command
if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand:
self.numberRepeatsRandomCommand = np.random.randint(maxTimesCommandRepeated + 1)
self.timesSameRandomCommandExecuted = 0
# Random number between 30 and 180
numberOfSteps = np.random.randint(30, 180)
self.lastRandomStepsWheelA = numberOfSteps
self.lastRandomStepsWheelB = numberOfSteps
# Random number between 0 and 1
randomNumber = np.random.random_sample()
if(randomNumber < 0.4):
if(randomNumber < 0.2):
self.moveForwards(numberOfSteps)
self.lastRandomCommand = self.moveForwards
else:
self.moveBackwards(numberOfSteps)
self.lastRandomCommand = self.moveBackwards
else:
# Random enums.Direction: left of right
if(np.random.random_sample() < 0.5):
direction = enums.Direction.forwards_left
else:
direction = enums.Direction.forwards_right
self.lastRandomDirection = direction
if(randomNumber < 0.7):
self.turnOnTheSpot(numberOfSteps, direction)
else:
self.turnOnTheSpot(numberOfSteps, direction)
self.lastRandomCommand = self.turnOnTheSpot
# Repeat last command
else:
self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1
# TODO: change wheel A/B!
if self.lastRandomCommand == self.moveForwards:
self.moveForwards(self.lastRandomStepsWheelA)
elif self.lastRandomCommand == self.moveBackwards:
self.moveBackwards(self.lastRandomStepsWheelA)
elif self.lastRandomCommand == self.turnOnTheSpot:
self.turnOnTheSpot(self.lastRandomStepsWheelA, self.lastRandomDirection)
def doRandomMovement(self):
# Random number between 30 and (509/4 + 30)
numberOfSteps = int(509/4*np.random.random_sample() + 30)
# Random number between 0 and 1
randomNumber = np.random.random_sample()
if(randomNumber < 0.4):
if(randomNumber < 0.2):
self.moveForwards(numberOfSteps)
else:
self.moveBackwards(numberOfSteps)
else:
# Random enums.Direction: left of right
if(np.random.random_sample() < 0.5):
direction = enums.Direction.forwards_left
else:
direction = enums.Direction.forwards_right
if(randomNumber < 0.7):
self.turnOnTheSpot(numberOfSteps, direction)
else:
self.turnOnTheSpot(numberOfSteps, direction)
def __init__(self, nodeId):
Sensors.__init__(self, nodeId)
self.temps = thermic.find_sensors()
def run(self):
sleep(5 * self.sleep_duration)
fuel_data = []
distance_data = []
engine_speed_data = []
gear_data = []
vehicle_speed_data = []
Sensors.get_last(
lambda obj: obj['name'] == 'fuel_consumed_since_restart',
fuel_data)
Sensors.get_last(lambda obj: obj['name'] == 'odometer', distance_data)
while True:
sleep(self.sleep_duration)
Sensors.get_last(
lambda obj: obj['name'] == 'fuel_consumed_since_restart',
fuel_data)
Sensors.get_last(
lambda obj: obj['name'] == 'odometer',
distance_data)
Sensors.get_last(
lambda obj: obj['name'] == 'engine_speed',
engine_speed_data)
Sensors.get_last(
lambda obj: obj['name'] == 'transmission_gear_position',
gear_data)
Sensors.get_last(
lambda obj: obj['name'] == 'vehicle_speed',
vehicle_speed_data)
if gear_data and engine_speed_data and vehicle_speed_data:
self._evaluateGearChange(engine_speed_data[-1].get('value'), gear_data[-1].get(
'value'), vehicle_speed_data[-1].get('value'), vehicle_speed_data[-1].get('timestamp'))
# difference between the two last measured fuel levels
fuel_diff = fuel_data[-1].get('value') - fuel_data[-2].get('value')
distance_diff = distance_data[-1].get('value') - \
distance_data[-2].get('value')
if distance_diff > 0:
fuel_usage_per_km = fuel_diff / distance_diff
else:
fuel_usage_per_km = 0
self.queue.put({'name': 'fuel_usage10',
'value': fuel_usage_per_km * 10,
'timestamp': fuel_data[-1].get('timestamp')})
