def main():
run(driver.Driver(50, 20, 25),
update,
view,
15,
quit_when=lambda s: s.quit or s.win,
final_view=final_view)
def _init_driver(self):
if not self._config.get('driver'):
return
self._driver = driver.Driver(self._driver)
self._driver.init()
self._on_driver_init()
def main():
pygame.init()
gamedisplay = pygame.display.set_mode((840, 650))
print("#testing driver car model#")
test_driver = driver.Driver(0, 0)
print("#right input test#")
test_driver.moveRight()
assert test_driver.x == 5
print('#zero input test#')
assert test_driver.x == 5
print('#left input test#')
test_driver.moveLeft()
assert test_driver.x == 0
print("#testing traffic model#")
test_traffic = traffic.Traffic(0)
print('#speedup test#')
oldspeed = test_traffic.randomspeed
test_traffic.speedup()
assert test_traffic.randomspeed > oldspeed
print('#slowdown test#')
oldspeed = test_traffic.randomspeed
test_traffic.slowdown()
assert test_traffic.randomspeed < oldspeed
print('#reset test#')
test_traffic.reset()
assert test_traffic.y >= -2000 and test_traffic.y <= -500
print('#drive test#')
test_traffic.drive()
assert test_traffic.y >= -2000 and test_traffic.y <= -500
print('#testing gameSetup model#')
test_gameSetup = gameSetup.GameSetup()
print('#game screen test#')
test_gameSetup.setup()
assert test_gameSetup.display_width == 840
print('#testing background model#')
test_background = background.Background()
print('#testing reset#')
test_background.reset()
assert test_background.y == -650
print('#testing speedup#')
test_background.speedup()
assert test_background.speed <= 5
print('#testing slowdown#')
test_background.slowdown()
assert test_background.speed >= 2
def handle_binary(text):
d = driver.Driver()
bs = binary.BinaryShifter(text)
while bs.shift():
bs.update_pattern()
time.sleep(.5)
def __init__(self, num_bulbs = None):
d = driver.Driver()
if (num_bulbs is None):
num_bulbs = 50
self.num_bulbs = num_bulbs
self.bulbs = []
for id in range(num_bulbs):
self.bulbs.append(bulb.Bulb(id, d))
self.broadcast = bulb.Bulb(BROADCAST, d)
def lauch_app(self, client_ip=None, client_port=None):
logging.info('Start function launch app calculator for mac')
self.obj_calculator = driver.Driver(client_ip=client_ip,
client_port=client_port,
app_name='Calculator')
self.calculator_driver = self.obj_calculator.get_driver()
self.assertTrue(self.calculator_driver, 'Get calculator driver failed')
logging.info('lauch_app successfully')
return True
def run(sc, table_A, table_B, candidate_pairs, table_G, feature_table, feature_info,
num_executors, seed_size, max_iter, batch_size):
# prepare driver
# driver node
driver = driver.Driver()
driver.prepare(table_A, table_B, feature_table, helper.tok_name2func,
helper.sim_name2func)
# seeds
seeder = seeder.Seeder(table_G)
labeler = labeler.Labeler(table_G)
# partition pairs
pair_rdd = sc.parallelize(candidate_pairs, num_executors)
bc_table_A = sc.broadcast(table_A)
bc_table_B = sc.broadcast(table_B)
bc_feature_info = sc.broadcast(feature_info)
# compute feature vectors
ex_rdd = pair_rdd.mapPartitions(
lambda pairs_partition: create_executors(pairs_partition, bc_table_A,
bc_table_B, bc_feature_info, num_executor, cache_level),
preservesPartitioning=True)
ex_rdd.cache()
# simulate active learning
# select seeds
pair2label = seeder.select(seed_size)
exclude_pairs = set(pair2label.keys())
num_iter = 0
all_features = set()
while num_iter<max_iter:
driver.add_new_training(pair2label)
# train model
rf = driver.train()
# features in RF
required_features = nodes.helper.get_features_in_random_forest(rf)
all_features.update(required_features)
# select most informative examples
candidates = ex_rdd.mapPartitions(
lambda executors: iteration(executors, rf, batch_size, exclude_pairs),
preservesPartitioning=True).collect()
# select top k from candidate
top_k = heapq.nlargest(batch_size, candidates, key=lambda p: p[1])
top_k_pairs = [ t[0] for t in top_k ]
pair2label = labeler.label(top_k_pairs)
exclude_pairs.update(top_k_pairs)
num_iter += 1
ex_rdd.unpersist()
def __init__(self):
self.sensors = sensors.Sensors()
self.map = map.Map(240, 240, 10)
self.api = api.API()
self.driver = driver.Driver()
self.turns = 0
self.direction = True
self.LANE_WIDTH = 12
self.FORWARD_DIST = 120
self.PROXIMITY_DIST = 10
self.MIN_UNVISITED_AREA = 10
def handle_ip():
d = driver.Driver()
wlan_ip = get_interface_ip('wlan1')
wlan_ip = '\x8d\xd4\x6e\xed'
bs = binary.BinaryShifter(wlan_ip)
while bs.shift():
bs.update_pattern()
time.sleep(.5)
def callmain(self):
if counter == 1:
global c1
global c2
global c3
global c4
c1 = int(self.v1.text())
c2 = int(self.v2.text())
c3 = int(self.v3.text())
c4 = int(self.v4.text())
board_coords = (c1, c2, c3, c4)
import driver
driver_obj = driver.Driver(board_coords)
driver_obj.play()
def setUp(self):
super(DriverTest, self).setUp()
self.afe = self.mox.CreateMock(frontend.AFE)
self.be = board_enumerator.BoardEnumerator(self.afe)
self.ds = deduping_scheduler.DedupingScheduler(self.afe)
self.mv = self.mox.CreateMock(manifest_versions.ManifestVersions)
self.config = forgiving_config_parser.ForgivingConfigParser()
self.nightly_bvt = task.Task(timed_event.Nightly.KEYWORD, '', '')
self.weekly_bvt = task.Task(timed_event.Weekly.KEYWORD, '', '')
self.new_build_bvt = task.Task(build_event.NewBuild.KEYWORD, '', '')
self.driver = driver.Driver(self.ds, self.be)
def pair_rider(ridereq):
dynamodb = boto3.resource('dynamodb')
table = dynamodb.Table('driver_queue')
#scan table for drivers
response = table.scan()
d_list = []
for driv in response['Items']:
d_id = driv['driver_id']
d_np = driv['num_passengers']
d_queue = driver.load(driv['queue'])
d_list.append(driver.Driver(d_id, d_np, d_queue))
return engine.BetterEngine(d_list, [ridereq])
def handle_website():
"""Handles Driver class from driver.py"""
# Initialize Driver class
driver = webdriver.Driver(
website='https://covid-19.ontario.ca/school-screening/', delay=10)
# Handle screening form
print('Opening website...')
driver.open_website()
print('Starting school screening...')
driver.start_school_screening()
print('Selecting student...')
driver.select_student()
print('Selecting continue...')
driver.select_continue(
'/html/body/div/div[1]/div[2]/main/div/div/div/div[2]/button')
for i in range(4):
print(f"No I have not #{i+1}...")
driver.select_no(
'/html/body/div/div[1]/div[2]/main/div/div/div/div/div[2]/button[1]'
)
print('Selecting continue again...')
driver.select_continue(
'/html/body/div/div[1]/div[2]/main/div/div/div/div/div[2]/button')
print('Selecting no...')
driver.select_no(
'/html/body/div/div[1]/div[2]/main/div/div/div/div/div[2]/button[1]')
# Take a screenshot of the verification page and name according to current date
print('Taking a screenshot...')
driver.screenshot(get_filename())
# Close browser
print('Closing the browser...')
driver.driver.close()
def read_file(filename):
with open(filename, 'r') as f:
line = f.readline()
rows, columns, totalVehicles, totalRides, bonusPerRide, totalTime = [
int(n) for n in line.split()
]
for i in range(totalVehicles):
drivers.append(driver.Driver(i))
for i in range(totalRides):
line = f.readline().split()
rides.append(
ride.Ride((int(line[0]), int(line[1])),
(int(line[2]), int(line[3])), int(line[4]),
int(line[5]), i))
rides.sort(key=lambda x: x.distance())
print(rides)
return rows, columns, totalVehicles, totalRides, bonusPerRide, totalTime
def __init__(self, lidar_on=True,small=True):
sensors_number=6
self.sensor_range = 20
self.collision_avoidance = False
if small:
self.sensors_map = {0:(0, np.pi/3),1: (np.pi*0.5, np.pi*1.5),2: (5/3.*np.pi,2*np.pi),3: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]}
#self.sensors_map= {0: (0, np.pi/3), 1: (np.pi/4, np.pi*7/12), 2: (np.pi*0.5, np.pi*1.5), 3: (17/12.*np.pi, 7/4.*np.pi), 4: (5/3.*np.pi,2*np.pi), 5: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]} # can be problem with 2pi and 0
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
if small:
self.coords = Array('d',[850, 170, 3*np.pi / 2])
else:
self.coords = Array('d', [170, 170, 0])
self.localisation = Value('b', True)
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500, sense_noise=25, distance_noise=45, angle_noise=0.2, in_x=self.coords[0],
in_y=self.coords[1], in_angle=self.coords[2],input_queue=self.input_queue, out_queue=self.loc_queue)
# driver process
self.dr = driver.Driver(self.input_queue,self.fsm_queue,self.loc_queue)
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,args=(self.localisation,self.coords,self.get_raw_lidar))
logging.info(self.send_command('echo','ECHO'))
logging.info(self.send_command('setCoordinates',[self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]]))
self.p2.start()
time.sleep(0.1)
def __init__(self):
#Run the game window setup
self.game = gameSetup.GameSetup()
self.game.setup()
#Initialize all needed objects
self.gamedisplay = self.game.gamedisplay
self.drivercar = driver.Driver(500, 400)
self.background = background.Background()
self.mainmenu = mainMenuLoop.TitleScreen(self.gamedisplay)
self.crashmenu = crashMenuLoop.CrashMenuLoop(self.gamedisplay)
self.lane1 = traffic.Traffic(110)
self.lane2 = traffic.Traffic(230)
self.lane3 = traffic.Traffic(370)
self.lane4 = traffic.Traffic(500)
self.slowfont = pygame.font.SysFont("monospace", 30)
self.scorefont = pygame.font.SysFont("monospace", 20)
self.dodged = 0
pygame.mixer.music.load("assets/race_music.mp3")
#Group the lane objects together
self.lanegroup = pygame.sprite.Group(self.lane1, self.lane2,
self.lane3, self.lane4)
handlers = [console_handler]
logging.basicConfig(level = logging.INFO,
format = '[%(levelname)s] [%(module)10s] %(message)s',
handlers = handlers
)
NUM_PEERS = 2
SIM_DURATION = 300
# create env
env = simpy.Environment()
# network
net = network.Network(env,2)
#create peers
nodes = []
teste = env.timeout(200)
for i in range (NUM_PEERS):
proc = processor.Processor(env, i, 3)
dri = driver.Driver(net, proc)
new_peer = peer.Peer(dri, i)
nodes.append(new_peer)
env.process(dri.run())
env.run(until=SIM_DURATION)
import create import driver from time import sleep import math driver = driver.Driver(0, 0, 0) driver.disable()
n_cars = 10 # NUMBER OF CARS PER GENERATION
#############################################################
#############################################################
################# START WORLD AND TRACK #####################
earth = w.World(pixels)
monza = t.Track(l.lane(lane_l))
t.map(monza,earth,pixels)
xmin, xmax, ymin, ymax = monza.min_Xcoor, monza.max_Xcoor, monza.min_Ycoor, monza.max_Ycoor
print("Space is between %d<%d and %d<%d."%(xmin,xmax,ymin,ymax))
#############################################################
#############################################################
################## FIRST CAR GENERATION #####################
cars = []
brains = []
drivers = []
for ii in range(n_cars):
cars.append(c.Car([0,0],np.random.randn(1),monza,earth,5+ii))
l = False
while l == False:
l = loc.locate_cars(np.mod(ii,4),cars[ii])
brains.append(n.Network(2,[6,10,10,2],False))
drivers.append(d.Driver(cars[ii],brains[ii]))
#############################################################
for t in range(1000):
for ii in range(n_cars):
if drivers[ii].crash == False:
drivers[ii].drive(deltaT, min_l)
def fun_loop(self):
#___________Model Car
Car_Settings = config.Car_Settings()
demeter = car.AwesomeCar(Car_Settings)
#initial Position
vel = 10
#___________Path_map
path_map_settings = config.Path_Map_Settings()
path_points = pm.path_map(path_map_settings)
path_points.createPoints()
path_points.createObstacles()
#__________Autonomous driver
drive_settings = config.Autonomous_Driver_Settings()
driver = dr.Driver(demeter, path_points, drive_settings)
#___________Path_plan
driver.plan.initialize(self.window, demeter.position,
demeter.direction)
run = True
driving_conditions = None
pygame.mixer.music.load(fun_song)
pygame.mixer.music.play(-1)
start_time = pygame.time.get_ticks()
while run:
pygame.time.delay(60) #Hz
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
keys = pygame.key.get_pressed()
if keys[pygame.K_q]:
pygame.mixer.music.stop()
pygame.quit()
quit()
if keys[pygame.K_m]:
pygame.mixer.music.stop()
run = False
self.game_intro()
if keys[pygame.K_LEFT]:
demeter.turn_left(drive_settings._velocity_)
if keys[pygame.K_RIGHT]:
demeter.turn_right(drive_settings._velocity_)
if keys[pygame.K_UP]:
demeter.move_forward(drive_settings._velocity_)
if keys[pygame.K_DOWN]:
demeter.move_backward(drive_settings._velocity_)
if keys[pygame.K_a]:
drive_settings._velocity_ += 1
if keys[pygame.K_d]:
drive_settings._velocity_ -= 1
if keys[pygame.K_a]:
drive_settings._velocity_ += 1
if keys[pygame.K_d]:
drive_settings._velocity_ -= 1
if keys[pygame.K_r]:
path_points.createPoints()
driver.plan.reset_checkpoints()
demeter.position['x'] = 50
demeter.position['y'] = 50
start_time = pygame.time.get_ticks()
path_points.createObstacles()
#___________________ Main simulation Loop _____________________#
self.window.blit(self.home_background_image, [0, 0])
path_points.draw_path(self.window)
demeter.draw_car(self.window)
driver.plan.update_plan(self.window,
demeter.position,
demeter.direction,
draw_plan=True,
debug=False)
rel_time = round((pygame.time.get_ticks() - start_time) / 1000, 4)
# #Display options
# self.display_text("Use arrows to create disturbance", simulation_text, white, (win_width -150), (30), False)
# self.display_text("Increase velocity (A)", simulation_text, white, (win_width -100), (50), False)
# self.display_text("Decrease velocity (D)", simulation_text, white, (win_width -100), (70), False)
# self.display_text("Restart Path points (R)", simulation_text, white, (win_width -110), (90), False)
#
# self.display_text("Menu (M)", simulation_text, white, (win_width -50), (win_height -40), False)
# self.display_text("Quit (Q)", simulation_text, white, (win_width -50), (win_height -20), False)
#
# #Display outputs
# self.display_text("Time running [sec]: ", simulation_text, white, 110, (win_height -100),False)
# self.display_text(str(rel_time), simulation_text, white, 230, (win_height -100),False)
# self.display_text("Current speed [px/sec] ", simulation_text, white, 125, (win_height -80),False)
# self.display_text(str(drive_settings._velocity_), simulation_text, white, 235, (win_height -80),False)
# att = round(math.degrees(driving_conditions['angle_to_turn']),4)
# self.display_text("Angles to turn: ", simulation_text, white, 90, (win_height -60),False)
# self.display_text(str(att), simulation_text, white, 230, (win_height -60),False)
# self.display_text("Direction to turn: ", simulation_text, white, 100, (win_height-40),False)
# self.display_text(driving_conditions['direction_to_turn'], simulation_text, white, 220, (win_height-40),False)
pygame.display.update()
pygame.quit()
def __init__(self,
lidar_on=True,
color='yellow',
sen_noise=20,
angle_noise=0.2,
dist_noise=45,
init_coord=[170, 170, 0]):
self.color = color
# collision settings
self.collision_avoidance = True
self.sensor_range = 60
self.map = np.load('npmap.npy')
self.sensors_places = [
np.pi / 2, np.pi / 2, 0, np.pi, 3 * np.pi / 2, 3 * np.pi / 2, 0,
np.pi, 0, 0
] # DIRECTION OF SENSORS
self.sensors_map = {
0: (np.pi / 6, np.pi / 2 + np.pi / 3),
1: (np.pi / 2 - np.pi / 3, np.pi * 5 / 6),
2: (0.0, 0.0 + np.pi / 3),
3: (np.pi - np.pi / 3, np.pi + np.pi / 3),
4: (3 * np.pi / 2 - np.pi / 3, 11 * np.pi / 6),
5: (np.pi - np.pi / 6, 3 * np.pi / 2 + np.pi / 3),
6: (0.0, 0.0 + np.pi / 3),
7: (np.pi - np.pi / 3, np.pi + np.pi / 3), # 8 IR sensors
8: (2 * np.pi - np.pi / 3, 2 * np.pi),
9: (2 * np.pi - np.pi / 3, 2 * np.pi)
} # doubling values
#6: (3*np.pi/2 + np.pi/4, 2*np.pi), 7:(np.pi/2-np.pi/3, np.pi*5/6): (np.pi)# 6 ir sensors ENDED
#7:(0, np.pi/4), 8:(3*np.pi/2 + np.pi/4, 2*np.pi), 9:(np.pi - np.pi/3, np.pi + np.pi/3)} # 2 us sensors
#can be problems ith 2*np.pi and 0
self.lidar_on = lidar_on
self.collision_d = 9
self.coll_go = False
# localisation settings
self.localisation = Value('b', True)
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
driver.PORT_SNR = '325936843235' # need change
# for test
x1, x2, y1, y2 = 1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
angle = np.pi / 2
start = [x1, y1, angle]
#
self.coords = Array('d', rev_field(init_coord, self.color)) # 170, 170
#self.coords = Array('d',rev_field([1000, 1100, np.pi],self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500,
sense_noise=sen_noise,
distance_noise=dist_noise,
angle_noise=angle_noise,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
self.dr = driver.Driver(self.input_queue,
self.fsm_queue,
self.loc_queue,
device=get_device_name())
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
self.p2.start()
self.init_manipulators()
time.sleep(0.1)
logging.info("Robot __init__ done!")
# cv2.imshow('TRANSMITTING VIDEO',frame)
key = cv2.waitKey(1) & 0xFF
if run == 0:
client_socket.close()
break
else:
server_socket.close()
client_socket.close()
global run
run = 1
######################################################################################################################################################
#setup pin numbers for DC motor
drive = driver.Driver()
######################################################################################################################################################
# Set up variables that will be used later
held = "NA"
two = 0
######################################################################################################################################################
# define dictionary for control logging
lognum = 1
df = [
] # create a list with the first entry being the starting conditions in dictionary
collect = 1
######################################################################################################################################################
cap = cv2.VideoCapture(0) #set up video object
ret, frame = cap.read() #read frame
images_folder = "/home/pi/Desktop/pictures/" #set picture directory
colab_filename = "/content/pictures/"
#!/usr/bin/python
# outputs all the IP addresses in binary on the lights
import binary
import driver
import rgb_strand
import socket
import fcntl
import struct
SIOCGIFADDR = 0x8915
d = driver.Driver()
bs = binary.BinaryShifter()
def get_interface_ip(ifname):
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
saddr = fcntl.ioctl(s.fileno(), SIOCGIFADDR,
struct.pack('256s', ifname[:15]))
return saddr[20:24]
out_str = ''
try:
wlan_ip = get_interface_ip('wlan1')
print socket.inet_ntoa(wlan_ip)
out_str = wlan_ip + '\x00'
except:
print 'uh oh'
logtrans=logtrans)
print("Done.")
td = timedelta(seconds=(dt[1] - dt[0]).total_seconds()) / (numframes + 1)
curdate = dt[0] + td
fn_pattern = driver.config_datasource.get("filenames", "pattern")
fn_ext = driver.config_datasource.get("filenames", "ext")
for i in xrange(len(R_ip)):
outfn = datetime.strftime(curdate, fn_pattern) + '.' + fn_ext
print("Saving result to %s...") % outfn,
sys.stdout.flush()
exporter(R_ip[i], os.path.join(output_path, outfn), geodata)
print("Done.")
curdate += td
if save_original == True:
if output_path != os.path.split(fn[0])[0]:
shutil.copy(fn[0], output_path)
if output_path != os.path.split(fn[1])[0]:
shutil.copy(fn[1], output_path)
driver = driver.Driver()
driver.parse_args()
driver.read_configs()
driver.run(worker, num_prev_precip_fields=1, exporter_method=exporter_method)
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
except socket.error, msg:
print 'Could not make a socket.'
sys.exit(-1)
# one second timeout
sock.settimeout(1.0)
shutdownClient = False
curEpisode = 0
verbose = False
d = driver.Driver(arguments.stage)
while not shutdownClient:
while True:
print 'Sending id to server: ', arguments.id
buf = arguments.id + d.init()
print 'Sending init string to server:', buf
try:
sock.sendto(buf, (arguments.host_ip, arguments.host_port))
except socket.error, msg:
print "Failed to send data...Exiting..."
sys.exit(-1)
try:
buf, addr = sock.recvfrom(1000)
import math
import serialSend
import pgp
import findPort
import lifesaver
import subprocess
import time
device_name = "/dev/ttyACM0"
width = 20
height = 4
text = []
curPos = 1
screen = driver.Driver(device_name, width, height)
screen.configSize(width, height)
screen.blinkCursor(True)
screen.clear()
def whipe():
text = []
curPos = 1
screen.clear()
def update():
screen.clear()
screen.println(text)
screen.setCursor(curPos % 20, math.trunc(curPos))
import numpy as np
import time
import util
import threading
import driver as drv
import engine as eng
import output as out
DONE = False
driver = drv.Driver()
print("GLB - Driver instantiated - " + util.pretty_time())
engine = eng.Engine()
print("GLB - Engine instantiated - " + util.pretty_time())
output = out.Output()
print("GLB - Output instantiated - " + util.pretty_time())
driver.start(engine)
print("GLB - Driver started - " + util.pretty_time())
engine.start(driver, output)
print("GLB - Engine started - " + util.pretty_time())
output.start(engine)
print("GLB - Output started - " + util.pretty_time())
driver.run()
def simulation_loop(self):
#___________Model Car
Car_Settings = config.Car_Settings()
demeter = car.AwesomeCar(Car_Settings)
#initial Position
vel = 10
#___________Path_map
path_map_settings = config.Path_Map_Settings()
path_points = pm.path_map(path_map_settings)
path_points.createPoints()
path_points.createObstacles()
#__________Autonomous driver
drive_settings = config.Autonomous_Driver_Settings()
driver = dr.Driver(demeter, path_points, drive_settings)
#___________Path_plan
driver.plan.initialize(self.window, demeter.position,
demeter.direction)
#Deviating sensors
driver.plan.create_sensor(100, 0, 'd', 'd_f')
driver.plan.create_sensor(80, 25, 'd', 'd_dl')
driver.plan.create_sensor(80, -25, 'd', 'd_dr')
driver.plan.create_sensor(60, 55, 'd', 'd_dl')
driver.plan.create_sensor(60, -55, 'd', 'd_dr')
#Safety sensors
driver.plan.create_sensor(30, 0, 's', 's_f')
driver.plan.create_sensor(30, 30, 's', 's_dl')
driver.plan.create_sensor(30, -30, 's', 's_dr')
driver.plan.create_sensor(30, 90, 's', 's_sr')
driver.plan.create_sensor(30, -90, 's', 's_sl')
# #back sensors
driver.plan.create_sensor(60, 180, 's', 's_b')
run = True
driving_conditions = None
start_time = pygame.time.get_ticks()
draw_sensors = False
draw_plan = False
while run:
pygame.time.delay(60) #Hz
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
keys = pygame.key.get_pressed()
if keys[pygame.K_q]:
pygame.mixer.music.stop()
pygame.quit()
quit()
if keys[pygame.K_m]:
pygame.mixer.music.stop()
run = False
self.game_intro()
if keys[pygame.K_a]:
drive_settings._velocity_ += 1
if keys[pygame.K_d]:
drive_settings._velocity_ -= 1
if keys[pygame.K_w]:
drive_settings._angle_allowed_error_ += 1
if keys[pygame.K_s]:
drive_settings._angle_allowed_error_ -= 1
if keys[pygame.K_LEFT]:
demeter.position['x'] -= 10
if keys[pygame.K_RIGHT]:
demeter.position['x'] += 10
if keys[pygame.K_UP]:
demeter.position['y'] -= 10
if keys[pygame.K_DOWN]:
demeter.position['y'] += 10
if keys[pygame.K_1]:
if draw_sensors == False:
draw_sensors = True
else:
draw_sensors = False
if keys[pygame.K_2]:
if draw_plan == False:
draw_plan = True
else:
draw_plan = False
if keys[pygame.K_r]:
path_points.createPoints()
driver.plan.reset_checkpoints()
demeter.position['x'] = 50
demeter.position['y'] = 50
start_time = pygame.time.get_ticks()
path_points.createObstacles()
#___________________ Main simulation Loop _____________________#
self.window.blit(self.home_background_image, [0, 0])
path_points.draw_path(self.window)
demeter.draw_car(self.window)
print(str(draw_plan))
response = driver.drive(self.window, draw_plan, draw_sensors)
rel_time = round((pygame.time.get_ticks() - start_time) / 1000, 4)
#Display options
# self.display_text("Use arrows to create disturbance", simulation_text, white, (win_width -150), (30), False)
# self.display_text("Increase velocity (A)", simulation_text, white, (win_width -100), (50), False)
# self.display_text("Decrease velocity (D)", simulation_text, white, (win_width -100), (70), False)
# self.display_text("Increase A. error (W)", simulation_text, white, (win_width -100), (90), False)
# self.display_text("Decrease A. error (S)", simulation_text, white, (win_width -100), (110), False)
# self.display_text("Restart Path points (R)", simulation_text, white, (win_width -110), (130), False)
#
# self.display_text("Menu (M)", simulation_text, white, (win_width -50), (win_height -40), False)
# self.display_text("Quit (Q)", simulation_text, white, (win_width -50), (win_height -20), False)
#
# #Display outputs
# self.display_text("Time running [sec]: ", simulation_text, white, 110, (win_height -120),False)
# self.display_text(str(rel_time), simulation_text, white, 230, (win_height -120),False)
# self.display_text("Current speed [px/sec] ", simulation_text, white, 125, (win_height -100),False)
# self.display_text(str(drive_settings._velocity_), simulation_text, white, 235, (win_height -100),False)
# self.display_text("Allowed angle error: ", simulation_text, white, 115, (win_height -80),False)
# self.display_text(str(drive_settings._angle_allowed_error_), simulation_text, white, 230, (win_height -80),False)
# att = round(math.degrees(driving_conditions['angle_to_turn']),4)
# self.display_text("Angles to turn: ", simulation_text, white, 90, (win_height -60),False)
# self.display_text(str(att), simulation_text, white, 230, (win_height -60),False)
# self.display_text("Direction to turn: ", simulation_text, white, 100, (win_height-40),False)
# self.display_text(driving_conditions['direction_to_turn'], simulation_text, white, 220, (win_height-40),False)
# self.display_text("Driver response: ", simulation_text, white, 100, (win_height-20),False)
# self.display_text(response, simulation_text, white, 250, (win_height-20),False)
pygame.display.update()
pygame.quit()
import math
import armcontrol
import cv2
import numpy as np
from picamera import PiCamera
from time import sleep
from camSet import clickImage
from camSet import processImage
from camSet import getDist1
from camSet import checkAlignment
from detectColor import findColor
from camSet import fillCol
camera = PiCamera()
camera.start_preview(fullscreen=False, window=(100, 20, 640, 480))
driver = driver.Driver(3, 1)
arm = armcontrol.ArmControl("/dev/ttyUSB1")
sleep(10)
arm.sendCmd("9")
sleep(2)
driver.driveTo(3, 2, 100)
clickImage()
crop_img = processImage()
color = findColor(crop_img)
print("color 1 : ", color)
driver.driveTo(3, 4, 100)
clickImage()
crop_img = processImage()
color = findColor(crop_img)
print("color 2 : ", color)
def main():
# setup
np.seterr(divide='raise')
# definition: never changed on actual driving
EYE_Y = 100 # mm
#BIRD = np.array([0, 2000, 1000])
THETA_W = 35 / 180 * pi
REDUCE = 1
F = 1 # mm
MAP_SIZE = (3500, 4900) # x, z
TILE_SIZE = 700
# initialization: never changed on actual driving, calculated by definition
WIDTH, HEIGHT = (np.array((640, 480)) / REDUCE).astype(int) # px
SCALE = WIDTH / 2 / tan(THETA_W / 2) / F # px / mm
CONTEXT = {
'F': F,
'WIDTH': WIDTH,
'HEIGHT': HEIGHT,
'SCALE': SCALE,
}
THETA_H = THETA_W / WIDTH * HEIGHT
CM_Z = EYE_Y / tan(THETA_H / 2)
BM_Y = TILE_SIZE
BIRD_Y = BM_Y / tan(THETA_H / 2)
BIRD_Z = BM_Y + CM_Z
BIRD = np.array([0, BIRD_Y, BIRD_Z])
LINES = np.load('map.3500x4900.npy') # (N, 2, 2)
LINES_XZ = LINES.reshape(len(LINES), 4) # (N, 4)
LINES_XYZ = np.insert(LINES_XZ, (1, 3), 0, axis=1) # (N, 6)
WM_LINES_MAP = LINES_XYZ.reshape(len(LINES_XYZ), 2, 3) # (N, 2, 3)
# debug
LINE_WIDTH = 3 # line width of img [px]
FIGSIZE = np.array((1 * 2, 1)) * 6 # (width, height) [inches]
FIGPOS = (
[0.00, 0.05, 0.40, 0.9], # [left_edge_pos, bottom_edge_pos,
[0.40, 0.05, 0.60, 0.9]) # width, height]
AFS = [ # list of affine matrix
to_affine(theta2r(np.array([90, 0, 0]) * pi / 180)),
to_affine(theta2r(np.array([0, 30, 0]) * pi / 180)),
np.diag([*([1.6] * 3), 1]),
np.array([
[1, 0, 0, 600],
[0, 1, 0, 2500],
[0, 0, 1, 1],
[0, 0, 0, 1],
]),
]
IS_TV = False
SHOWN_MAP = ['TV', 'CAMERA', 'MAP', 'NONE'][0]
SHOWN_IMG = ['TV', 'CAMERA'][0]
IS_REAR = False
D_EYE_CAM_F = 90 # mm
D_EYE_CAM_R = 120 # mm
DRIVER = driver.Driver(
IS_PID=False,
IS_KEEPLEFT=True,
IS_KALMAN=True,
IS_DETECT_COURSEOUT=False,
IS_SIMULATION=True,
)
XINI = DRIVER.fixed_param['xini']
BIRD = DRIVER.fixed_param['tv_pos']
x, z, theta_y = XINI['x'], XINI['z'], XINI['theta']
eye = np.array([x, EYE_Y, z])
theta = np.array([0, theta_y, 0]) # 0 <= theta_x <= pi / 2
r = theta2r(theta)
uv_lines = wm_lines2uv_lines(WM_LINES_MAP, eye, r, CONTEXT)
dargs = Manager().dict({
# modified by func_update
'eye': eye,
'r': r,
'uv_lines': uv_lines,
# const
'CONTEXT': CONTEXT,
'BIRD': BIRD,
'WM_LINES_MAP': WM_LINES_MAP,
'MAP_SIZE': MAP_SIZE,
'LINE_WIDTH': LINE_WIDTH,
'FIGSIZE': FIGSIZE,
'FIGPOS': FIGPOS,
'AFS': AFS,
'EYE_Y': EYE_Y,
'FUNC_UPDATE': FUNC_UPDATE,
'INIT_DARGS': INIT_DARGS,
'IS_TV': IS_TV,
'SHOWN_MAP': SHOWN_MAP,
'SHOWN_IMG': SHOWN_IMG,
'IS_REAR': IS_REAR,
'DRIVER': DRIVER,
'D_EYE_CAM_F': D_EYE_CAM_F,
'D_EYE_CAM_R': D_EYE_CAM_R,
})
debug(dargs)
