def __init__(self, FPS, x, y, sprite, spriteLaser):
Player.__init__(self, x, y, sprite)
self.soundLaser = pygame.mixer.Sound(os.path.join("sounds", "laser.ogg"))
self.soundLaser.set_volume(0.5)
self.leftCTRL_UP = False
# the player can only have 1 laser active, so one 1 is every created.
self.laser = Laser(self.x, self.y - self.spriteCentre.height / 2, spriteLaser)
self.score = 0
# player will start game alive
self.alive = True
self.lives = 2
# every time a screen is cleared this will be incremented.
# this effects how many galaxians can dive at once and the time delay between galaxians launching.
self.level = 0
# wait 4 seconds to reinitialise the player or decide the game is over
self.waitTime = FPS * 4
# this timer is only decremented once the player is NOT alive
self.waitTimer = self.waitTime
self.spriteLife = pygame.image.load(os.path.join("images", "rocketShipLife.png")).convert_alpha()
self.spriteLifeRect = self.spriteLife.get_rect()
def __init__(self):
print "Set mode BCM"
config = json.load(open("config.json"))
GPIO.setmode(GPIO.BCM)
self.motorX = Motor(config["motor_x"]["pins"])
self.motorX.name = "X"
self.motorX.delay = config["motor_x"]["delay"]
self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]
self.motorY = Motor(config["motor_y"]["pins"])
self.motorY.name = "Y"
self.motorY.delay = config["motor_y"]["delay"]
self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]
self.laser = Laser(config["laser"]["pin"])
def connect_laser(self):
with self.lock:
self.laser = Laser()
self.connected.set(True)
class Model:
def __init__(self):
self.frequency = Observable()
self.power = Observable(0)
self.offset = Observable(0)
self.on = Observable(False)
self.connected = Observable(False)
self.clean_jump_active = Observable(False)
self.clean_scan_active = Observable(False)
self.clean_scan_progress = Observable()
self.clean_sweep_state = Observable()
self.scan_data = Observable({
't_laser': [],
'f': [],
'p_in': [],
't_pm': [],
'p_out': []
})
self.scan_data_old = Observable({'f': [], 't': []})
self.scan_time_remaining = Observable()
self.scan_update_active = Observable(False)
self.lock = Lock()
self.data_lock = Lock()
self.laser = None
self.power_meter = None
self.power_meter_connected = Event()
self.power_meter_connected.clear()
self.power_meter_thread = None
self.power_meter_stop = Event()
def set_startup_frequency(self, frequency):
self.frequency.set(frequency)
def connect_laser(self):
with self.lock:
self.laser = Laser()
self.connected.set(True)
def connect_pm(self):
rm = visa.ResourceManager()
resources = rm.list_resources()
print(resources)
inst = rm.open_resource(resources[0])
inst.timeout = 5000
print(inst.query('*IDN?'))
self.power_meter = ThorlabsPM100(inst=inst)
self.power_meter_connected.set()
def laser_on(self):
with self.lock:
self.laser.startup_begin(self.frequency.get())
while self.laser.check_nop() > 16:
self.power.set(self.laser.check_power())
self.on.set(True)
def laser_off(self):
# with self.lock:
self.laser.laser_off()
self.power.set(0)
self.on.set(False)
def disconnect(self):
with self.lock:
if self.laser:
self.laser.itla_disconnect()
self.connected.set(False)
def standard_update(self, gui_lock: Lock, update_stop_event: Event):
while not update_stop_event.is_set():
time.sleep(0.01)
with self.lock:
if not update_stop_event.is_set():
self.power.set(self.laser.check_power())
freq_thz = self.laser.read(Laser.REG_FreqTHz)
freq_ghz = self.laser.read(Laser.REG_FreqGHz)
self.frequency.set(freq_thz + freq_ghz / 10000)
self.offset.set(self.laser.offset())
def scan_update(self, end_event: Event, take_data: Event):
self.power_meter_connected.wait()
print('PM connected')
t_prev = None
p_prev = None
f_prev = None
stop_pm_event = Event()
power_queue = Queue()
time_queue = Queue()
pm_read_thread = Thread(target=self.pm_read,
args=(time_queue, power_queue, stop_pm_event))
pm_read_thread.start()
self.scan_update_active.set(True)
while not end_event.is_set():
queue = Queue(maxsize=1)
laser_read_thread = Thread(target=self.laser_read, args=(queue, ))
laser_read_thread.start()
output_powers_list = []
measured_time_list = []
laser_read_thread.join()
stop_pm_event.set()
pm_read_thread.join()
while not time_queue.empty():
measured_time_list.append(time_queue.get())
while not power_queue.empty():
output_powers_list.append(power_queue.get())
pm_read_thread = Thread(target=self.pm_read,
args=(time_queue, power_queue,
stop_pm_event))
stop_pm_event.clear()
pm_read_thread.start()
output_powers = np.array(output_powers_list)
measured_time = np.array(measured_time_list)
p_new, offset, t_end = queue.get()
self.power.set(p_new)
self.offset.set(offset)
f_new = self.frequency.get() + offset / 1000
if t_prev and f_prev and p_prev:
assert isinstance(f_prev, float)
assert isinstance(p_prev, float)
t_duration = t_end - t_prev
interpolation = ((measured_time - t_prev) / t_duration)
frequencies = list(interpolation * f_new +
(1 - interpolation) * f_prev)
input_powers = interpolation * p_new + (1 -
interpolation) * p_prev
input_powers_watts = np.power(10, input_powers / 10) / 1000
relative_powers = list(output_powers / input_powers_watts)
if abs(offset) < 20 and abs(p_new - 10) < 0.03 and abs(
f_prev - f_new) < 0.1 and take_data.is_set():
with self.data_lock:
data = self.scan_data_old.get()
data['f'] += frequencies
data['t'] += relative_powers
self.scan_data_old.set(data)
t_prev = t_end
f_prev = f_new
p_prev = p_new
self.scan_update_active.set(False)
def laser_read(self, queue: Queue):
with self.lock:
input_power = self.laser.check_power()
offset = self.laser.offset()
t = time.perf_counter()
results = input_power, offset, t
queue.put(results)
def pm_read(self, time_queue: Queue, power_queue: Queue, stop: Event):
while not stop.is_set():
power_queue.put(self.power_meter.read)
time_queue.put(time.perf_counter())
def pm_update(self, end_event: Event, take_data: Event):
self.power_meter_connected.wait()
while not end_event.is_set():
if take_data.is_set():
power_out = self.power_meter.read
with self.data_lock:
data = self.scan_data.get()
if len(data['t_laser']) > 1 and abs(
data['t_laser'][-1] - time.perf_counter()) < 2E-1:
data['t_pm'].append(time.perf_counter())
data['p_out'].append(power_out)
self.scan_data.set(data)
def clean_jump(self, frequency):
self.clean_jump_active.set(True)
self.clean_sweep_stop()
with self.lock:
self.laser.wait_nop()
power_reference = self.power.get()
with self.lock:
self.laser.clean_jump_start(frequency)
# Read the frequency error and wait until it is below a threshold or 2 seconds passes
wait_time = time.perf_counter() + 2
with self.lock:
error_read = self.laser.read(Laser.REG_Cjumpoffset)
freq_error = error_read / 10.0
self.offset.set(freq_error)
while abs(freq_error) > 0.1 and time.perf_counter() < wait_time:
with self.lock:
error_read = self.laser.read(Laser.REG_Cjumpoffset)
freq_error = error_read / 10.0
self.offset.set(freq_error)
with self.lock:
self.laser.wait_nop()
# Read out the laser's claimed frequency
with self.lock:
claim_thz = self.laser.read(Laser.REG_GetFreqTHz)
claim_ghz = self.laser.read(Laser.REG_GETFreqGHz) / 10
claim_freq = claim_thz + claim_ghz / 1000.0
self.frequency.set(claim_freq)
print('Laser\'s claimed frequency: %f' % claim_freq)
with self.lock:
self.laser.clean_jump_finish()
time_wait = time.perf_counter() + 1
while self.power.get() < 0.8 * power_reference and time.perf_counter(
) < time_wait:
time.sleep(.1)
self.clean_sweep_start(0, 1)
self.clean_jump_active.set(False)
def clean_sweep_start(self, frequency, speed):
self.clean_sweep_state.set("Preparing for clean sweep...")
with self.lock:
self.laser.clean_sweep_prep(frequency, int(speed * 1000))
time.sleep(1)
self.clean_sweep_state.set("Running clean sweep.")
with self.lock:
self.laser.clean_sweep_start()
def clean_sweep_stop(self):
with self.lock:
self.laser.clean_sweep_stop()
self.clean_sweep_state.set(None)
def clean_sweep_to_offset(self, offset):
self.clean_sweep_state.set(
"Sweeping to offset of {} GHz".format(offset))
with self.lock:
self.laser.clean_sweep_to_offset(offset)
while abs(self.offset.get() - offset) > 0.1:
time.sleep(0.1)
self.clean_sweep_state.set(
"Pausing sweep at offset of {} GHz".format(offset))
def clean_scan(self, start_frequency: float, stop_frequency: float,
speed: float, stop: Event, take_data: Event):
assert stop_frequency > start_frequency
if not self.power_meter_connected.is_set():
self.connect_pm()
jump_frequency = start_frequency
self.clean_scan_active.set(True)
self.clean_scan_progress.set(0)
self.scan_data.set({
't_laser': [],
'f': [],
'p_in': [],
't_pm': [],
'p_out': []
})
self.scan_data_old.set({'f': [], 't': []})
take_data.clear()
# self.power_meter_thread = Thread(target=self.pm_update, args=(stop, take_data))
# self.power_meter_thread.start()
scan_start_time = time.perf_counter()
scan_count = 0
while jump_frequency < stop_frequency + 0.03 and not stop.is_set():
power_reference = self.power.get()
with self.lock:
self.laser.clean_jump(jump_frequency)
freq_thz = self.laser.read(Laser.REG_FreqTHz)
freq_ghz = self.laser.read(Laser.REG_FreqGHz)
frequency = freq_thz + freq_ghz / 10000
self.frequency.set(frequency)
with self.lock:
self.laser.wait_nop()
time_wait = time.perf_counter() + 1
while self.power.get(
) < 0.8 * power_reference and time.perf_counter() < time_wait:
time.sleep(.1)
time.sleep(0.5)
power_reference = self.power.get()
self.clean_sweep_start(50, speed)
time.sleep(.1)
take_data.set()
while self.offset.get() < 10 and not stop.is_set():
time.sleep(.1)
while self.offset.get() > -10 and not stop.is_set():
time.sleep(.1)
while self.offset.get() < -1 and not stop.is_set():
time.sleep(0.1)
take_data.clear()
time.sleep(0.5)
with self.lock:
self.laser.clean_sweep_stop()
with self.lock:
self.laser.wait_nop()
time_wait = time.perf_counter() + 5
while self.power.get(
) < 0.95 * power_reference and time.perf_counter() < time_wait:
time.sleep(.1)
jump_frequency += 0.04
progress = (jump_frequency - start_frequency) / (stop_frequency -
start_frequency)
self.clean_scan_progress.set((jump_frequency - start_frequency) /
(stop_frequency - start_frequency))
time_elapsed = time.perf_counter() - scan_start_time
scan_count += 1
average_time_elapsed = time_elapsed / scan_count
self.scan_time_remaining.set(
round(average_time_elapsed / progress / 60))
time.sleep(1)
# self.power_meter_thread.join()
self.clean_scan_active.set(False)
from navigator import Navigator
from driver import Driver
from laser import Laser
from gyro import Gyro
from ir import IR
from communicator import Communicator
from eventbus import EventBus
from position import Position
from protocol import *
from safety import Safety
ir = IR()
laser = Laser()
gyro = Gyro()
driver = Driver(gyro, laser)
navigator = Navigator(Navigator.MANUAL, ir, driver, laser)
position = Position(laser, ir, navigator)
communicator = Communicator(ir, laser, gyro, driver, navigator, position)
def setup():
Safety.setup_terminal_abort()
EventBus.subscribe(BT_REQUEST_SENSOR_DATA, communicator.send_sensor_data)
EventBus.subscribe(BT_REQUEST_SERVO_DATA, communicator.send_servo_data)
EventBus.subscribe(BT_REQUEST_MAP_DATA, communicator.send_map_data)
EventBus.subscribe(BT_DRIVE_FORWARD, communicator.drive_forward)
EventBus.subscribe(BT_DRIVE_BACK, communicator.drive_backward)
EventBus.subscribe(BT_TURN_RIGHT, communicator.turn_right)
class AsteroidsGame(Widget):
spaceship = ObjectProperty(None)
laser = Laser()
laser2 = Laser()
laser3 = Laser()
asteroid = Asteroid()
asteroid2 = Asteroid()
count = 0
im_too_lazy_to_think_this_through = True
def __init__(self, **kwargs):
super(AsteroidsGame, self).__init__(**kwargs)
self._keyboard = Window.request_keyboard(self._keyboard_closed, self)
self._keyboard.bind(on_key_down=self._on_keyboard_down)
def _keyboard_closed(self):
self._keyboard.unbind(on_key_down=self._on_keyboard_down)
self._keyboard = None
def _on_keyboard_down(self, keyboard, keycode, text, modifiers):
# MOVEMENT
# Angle, in radians, of the spaceship
spaceship_angle = radians(self.spaceship.angle)
# Reversed cosine and sine to make 0 radians point/move up and to make
# movement feel more natural
if keycode[1] == 'up':
self.spaceship.x_vel -= sin(spaceship_angle)
self.spaceship.y_vel += cos(spaceship_angle)
if keycode[1] == 'down':
self.spaceship.x_vel += sin(spaceship_angle)
self.spaceship.y_vel -= cos(spaceship_angle)
# Rotation
if keycode[1] == 'right':
self.spaceship.turning_vel -= 1.5
if keycode[1] == 'left':
self.spaceship.turning_vel += 1.5
# Shooting
if keycode[1] == 'spacebar':
if self.laser.pos == [1600, 1200]:
self.laser.center = self.spaceship.center
self.laser.x_vel = -(sin(radians(self.spaceship.angle))) * 25
self.laser.y_vel = cos(radians(self.spaceship.angle)) * 25
# PAUSING AND RESUMING
if keycode[1] == 'p':
self.pause_toggle = not self.pause_toggle
return True
def spawn_asteroid(self):
random_coordinates = [
randint(0, self.width),
randint(0, self.height)
]
self.asteroid.velocity = Vector(4, 0).rotate(randint(0, 360))
self.asteroid.pos = random_coordinates
self.asteroid2.velocity = Vector(4, 0).rotate(randint(0, 360))
self.asteroid2.pos = random_coordinates
def check_player_lives(self):
if self.spaceship.lives <= 0:
#Stop game
#'GAME OVER'
#play again?
pass
def update(self, dt):
# Romove laser after 60/100ths of a second
if self.laser.x_vel != 0 or self.laser.y_vel != 0:
self.count += 1
if self.count > (60 * 0.6):
self.count = 0
self.laser.pos = -100, -100
self.laser.x_vel = 0
self.laser.y_vel = 0
# Allow movement
self.asteroid.move()
self.asteroid2.move()
self.laser.move()
self.laser2.move()
self.laser3.move()
self.spaceship.move()
# Spawn asteroid
if self.im_too_lazy_to_think_this_through:
self.spawn_asteroid()
self.im_too_lazy_to_think_this_through = False
# Handle laser-asteroid collision
self.laser.hit_asteroid(self.asteroid, self.spaceship)
self.laser.hit_asteroid(self.asteroid2, self.spaceship)
# Handle player-asteroid collision
self.asteroid.hit_player(self.spaceship)
self.spaceship.hit_asteroid(self.asteroid)
self.asteroid2.hit_player(self.spaceship)
self.spaceship.hit_asteroid(self.asteroid2)
# FRICTION LOGIC
# Slow down the spaceship movement over time
# Divide by 60 to compensate for x60 update rate
# UPWARD VELOCITY // POSITIVE y_vel
if self.spaceship.y_vel > 0:
self.spaceship.y_vel -= (self.spaceship.y_vel / 2) / 60
# RIGHT VELOCITY // POSITIVE x_vel
if self.spaceship.x_vel > 0:
self.spaceship.x_vel -= (self.spaceship.x_vel / 2) / 60
# Rotation
if self.spaceship.turning_vel < 0:
self.spaceship.turning_vel -= (self.spaceship.turning_vel * 2) / 60
# DOWNWARD VELOCITY // NEGATIVE y_vel
if self.spaceship.y_vel < 0:
self.spaceship.y_vel += -(self.spaceship.y_vel / 2) / 60
# LEFT VELOCITY // NEGATIVE x_vel
if self.spaceship.x_vel < 0:
self.spaceship.x_vel += -(
self.spaceship.x_vel / 2) / 60
# Rotation
if self.spaceship.turning_vel > 0:
self.spaceship.turning_vel -= (
self.spaceship.turning_vel * 2) / 60
# SCREEN COLLISION
# Warp objects on screen collision
transition_offset = -10
# BOTTOM
if self.spaceship.top < 0:
self.spaceship.y = self.height
if self.laser.top < 0:
self.laser.y = self.height
if self.asteroid.top < 0:
self.asteroid.y = self.height
if self.asteroid2.top < 0:
self.asteroid2.y = self.height
# TOP
if self.spaceship.y > self.height:
self.spaceship.top = 0
if self.laser.y > self.height:
self.laser.top = 0
if self.asteroid.y > self.height:
self.asteroid.top = 0
if self.asteroid2.y > self.height:
self.asteroid2.top = 0
# LEFT
if self.spaceship.right < transition_offset:
self.spaceship.x = self.width
if self.laser.right < transition_offset:
self.laser.x = self.width
if self.asteroid.right < transition_offset:
self.asteroid.x = self.width
if self.asteroid2.right < transition_offset:
self.asteroid2.x = self.width
# RIGHT
if self.spaceship.x > self.width:
self.spaceship.right = 0
if self.laser.x > self.width:
self.laser.right = 0
if self.asteroid.x > self.width:
self.asteroid.right = 0
if self.asteroid2.x > self.width:
self.asteroid2.right = 0
def play(self):
self.p1.status_win.display(
curses.COLS // 2 - self.ship_space - self.max_health,
2
)
self.p2.status_win.display(
curses.COLS // 2 + self.ship_space,
2
)
if self.p1.scene.objects[self.p1.stacking_order].x2 != self.midpoint_x - self.ship_space:
self.main_win.move_to(self.p1, x=self.midpoint_x - self.ship_space - self.p1.width)
self.main_win.pan(x=self.main_win.width - self.main_win.win_width)
laser = Laser(self.p1, self.p2, self.main_win.height // 2)
self.main_win.add_object(laser, laser.x1, laser.y)
shield1 = Drawable(')')
shield1.set_color(curses.COLOR_CYAN)
shield1.set_attrs(curses.A_BOLD)
self.main_win.add_object(shield1, laser.x1, laser.y)
self.main_win.hide(shield1)
shield2 = Drawable('(')
shield2.set_color(curses.COLOR_CYAN)
shield2.set_attrs(curses.A_BOLD)
self.main_win.add_object(shield2, laser.x2, laser.y)
self.main_win.hide(shield2)
self.main_win.refresh()
self.main_win.auto_refresh = True
step = 0
while not self.p1.health.is_empty() and not self.p2.health.is_empty():
time.sleep(0.5)
target1 = self.p1.get_target()
target2 = self.p2.get_target()
target = None
successful = True
if step < 5:
target = self.p1
if not self.is_valid_target(target1):
target1 = 0
elif not self.is_valid_target(target2) or target1 == target2:
successful = False
else:
target = self.p2
if not self.is_valid_target(target2):
target2 = 0
elif not self.is_valid_target(target1) or target1 == target2:
successful = False
self.p1.show_target(target2)
self.p2.show_target(target1)
direction = 1
if target is self.p1:
direction = -1
self.main_win.show(laser)
laser.start(direction)
time.sleep(0.01)
while laser.advance():
time.sleep(0.02)
if not successful:
if direction > 0:
self.main_win.show(shield2)
time.sleep(0.5)
self.main_win.hide(shield2)
else:
self.main_win.show(shield1)
time.sleep(0.5)
self.main_win.hide(shield1)
self.main_win.hide(laser)
if successful:
target.health.decrease()
target.set_color(curses.COLOR_RED)
if target.health.is_empty():
continue
time.sleep(0.2)
target.set_color(curses.COLOR_WHITE)
step = (step + 1) % 10
explosion = Explosion(target)
explosion.set_color(curses.COLOR_YELLOW)
explosion.set_attrs(curses.A_BOLD)
self.main_win.add_object(explosion, explosion.x, explosion.y)
explosion.explode()
self.winner = self.p2
if target is self.p2:
self.winner = self.p1
class RocketShip(Player):
def __init__(self, FPS, x, y, sprite, spriteLaser):
Player.__init__(self, x, y, sprite)
self.soundLaser = pygame.mixer.Sound(os.path.join("sounds", "laser.ogg"))
self.soundLaser.set_volume(0.5)
self.leftCTRL_UP = False
# the player can only have 1 laser active, so one 1 is every created.
self.laser = Laser(self.x, self.y - self.spriteCentre.height / 2, spriteLaser)
self.score = 0
# player will start game alive
self.alive = True
self.lives = 2
# every time a screen is cleared this will be incremented.
# this effects how many galaxians can dive at once and the time delay between galaxians launching.
self.level = 0
# wait 4 seconds to reinitialise the player or decide the game is over
self.waitTime = FPS * 4
# this timer is only decremented once the player is NOT alive
self.waitTimer = self.waitTime
self.spriteLife = pygame.image.load(os.path.join("images", "rocketShipLife.png")).convert_alpha()
self.spriteLifeRect = self.spriteLife.get_rect()
def move(self, gal, screenWidth):
# updates the sprite and hit rectangle
Player.move(self)
if self.alive:
userInput = pygame.key.get_pressed()
if userInput[pygame.K_LEFT] and self.alive:
self.x -= 3
if userInput[pygame.K_RIGHT] and self.alive:
self.x += 3
if userInput[pygame.K_LCTRL] and self.leftCTRL_UP and not self.laser.active:
self.laser.active = True
self.soundLaser.play()
# stop the player moving off the screen
self.x = limits(self.x, 0, screenWidth, self.spriteCentre.width / 2)
# prevents auto fire
self.leftCTRL_UP = not userInput[pygame.K_LCTRL]
# update the laser, regardless if it is active or not
self.laser.move(self.x, gal)
# game is not over, return false
return False
else:
self.sprite = self.spriteBlank
if self.laser.active:
self.laser.move(self.x, gal)
else:
# just keep it out of sight for now
self.laser.sprite = self.laser.spriteBlank
state = States()
# wait until all galaxians are back in formation before setting the player back up
for f in range(len(gal)):
if not gal[f].state == state.FORMATION:
# the players lives might be < 0, but return false until all galaxians are back in formation
return False
if self.waitTimer > 0:
self.waitTimer -= 1
if self.waitTimer == 0:
self.lives -= 1
if self.lives >= 0:
self.alive = True
self.x = screenWidth / 2
self.laser.x = self.x
self.waitTimer = self.waitTime
self.sprite = self.spriteStart
self.laser.sprite = self.laser.spriteStart
# player still has lives
return False
else:
# game over now!
return True
def draw(self, window, screenHeight):
Player.draw(self, window)
self.laser.draw(window)
for f in range(self.lives):
window.blit(self.spriteLife, (10 + f * (self.spriteLifeRect.width + 5),
screenHeight - self.spriteLifeRect.height))
width = 617
offset_x =0.0
offset_y =0.0
resolution = 0.1
# Create map and laser scans
occ_map = Map.readFromTXT('../map.txt', width, height, offset_x, offset_y, resolution)
max_range = 50.0
no_of_beams = 181
min_angle = -math.pi/2.0
resolution_angle = math.pi/(no_of_beams-1)
noise_variance = 0.0
laser_pos_x = 1.2
laser_pos_y = 0.0
laser_angle = 0.0 * (math.pi/180.0)
laser = Laser(max_range, min_angle, resolution_angle, no_of_beams, noise_variance, occ_map, laser_pos_x, laser_pos_y, laser_angle)
# Read positions
positions = np.loadtxt('../data_pose.txt')
# Read corresponding laser scans
#laser_scans = np.loadtxt('../map_scans.txt') #, delimiter=','
# Go through each position and generate a new scan
# Validate each generated scan against recorded ones
counter = 0
all_ranges = []
n = 2000
for i in range(n):
ranges = laser.scan(positions[i,0], positions[i,1], positions[i,2])
all_ranges.append(copy(ranges))
def __init__(self, mrds):
self.linear = None
self.angular = None
self.mrds = mrds
self.laser = Laser(mrds)
return
class Movement:
def __init__(self, memap):
self.move = rospy.Publisher("/cmd_vel", Twist,
queue_size=1) #entender bem esta linha
self.speed = Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0))
self.angular = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0.5))
self.memap = memap
self.laser = Laser()
#self.abort_and_survive
def stop(self):
print("ASDASD")
self.move.publish(Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0,
0.0)))
def update(self):
self.laser.getClosest()
readings = self.laser.getClosest()
#velocidade = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
#velocidade_saida.publish(velocidade)
minimum_distance = 0.4
if (readings[1] < minimum_distance) and ((readings[0] <= 40) or
(readings[0] >= 320)):
#Emergência! Algo entrou no caminho!
print("algo esta proximo demais! :o")
#print("/t detectado ruindade em: "+str(readings))
if readings[
0] <= 40: # 45 graus fica a esquerda dele? então roda para a direita
print("objeto a esquerda manobras evasivas para a direita!")
mov = (Twist(Vector3(-0.5, 0, 0), Vector3(0, 0, 1)))
elif readings[
0] >= 320: # 315 graus fica a direita dele? então ele roda para a esquerda
print("objeto a direita manobras evasivas para a esquerda!")
mov = (Twist(Vector3(-0.5, 0, 0), Vector3(0, 0, -1)))
print(mov)
self.move.publish(mov)
# else:
# self.move.publish(Twist(Vector3(0.1,0,0), Vector3(0,0,2))) # não é mais necessario,espero
elif (self.memap.triangle is None or self.memap.triangle.age >= 10):
#Temos que procurar o triângulo!
print("cadê triângulo? :c")
self.move.publish(self.angular)
else:
#Achamos o triângulo!
print("triângulo achado! ATACAR! >:3")
pos = self.memap.triangle.center
resolution = self.memap.resolution
turnSpeed = 1.0
mov = Twist(Vector3(0.5, 0, 0), Vector3(0, 0, turnSpeed))
distance = pos[0] - (
resolution[0] / 2
) #recebe x do objeto e vê se está a direita ou esquerda da tela
angular = turnSpeed * (float(distance / (resolution[0] / 2)))
mov = Twist(Vector3(0.5, 0, 0), Vector3(0, 0, angular))
self.move.publish(mov)
def fire_laser(ai_settings, screen, ship, lasers):
"""Fires deadly lasers"""
if len(lasers) < ai_settings.lasers_allowed:
new_laser = Laser(ai_settings, screen, ship)
lasers.add(new_laser)
def key_down_action(key):
if key == pygame.K_s:
laser = Laser(ship)
lasers.append(laser)
move_ship(key)
class PathTracker:
"""
Uses Pure Pursuit to determine next position in path to take.
Chooses point based on observability and collision detection.
"""
def __init__(self, path, communicator):
self._path = path
self._laser = Laser(communicator)
def get_turn_radius_inverse(self, robot_x, robot_y, robot_angle):
"""
Get radius inverse, also called gamma, for the circle based upon the
robots coordinates a goal point which is chosen.
Will raise EndOfPathError if robot is within 1 m of end of path
"""
try:
path_x, path_y = self.get_next_point(robot_x,robot_y,robot_angle)
except EndOfPathError:
x, y = self._path.get_last_position()
path_x, path_y = utils.translate_coordinates_between_systems(x, y,
robot_x, robot_y, robot_angle)
if utils.distance_between_two_points(0, 0, path_x, path_y) < 1:
raise EndOfPathError('Within 1m of end of path')
if not self._laser.check_if_circle_safe(path_x,path_y):
while True:
x, y = self._path.previous()
translated_x, translated_y = \
utils.translate_coordinates_between_systems(x, y,
robot_x, robot_y, robot_angle)
if self._laser.check_if_circle_safe(translated_x,
translated_y):
path_x, path_y = translated_x, translated_y
break
try:
self._path.previous()
except EndOfPathError:
pass
return self.get_turn_radius_inverse_to_point(path_x, path_y)
def get_turn_radius_inverse_to_point(self, x, y):
"""Performs Pure Pursuit Algorithm to calculate radius inverse"""
L_adjusted = utils.distance_between_two_points(0, 0, x, y)
angle = utils.angle_between_two_points(0, 0, x, y)
if abs(angle) > math.pi/2:
return 9999999*utils.sign(angle)
if abs(angle) > math.pi/2:
angle = utils.sign(angle) * math.pi/2
if abs(angle) == math.pi/2:
angle -= utils.sign(angle)*0.0001
gamma = (2*y)/(L_adjusted**2)
return gamma
def get_next_point(self, robot_x, robot_y, robot_angle):
"""
Get next point to which is viable to travel to. Will base
viability on that point must be visible and able to be
traveled to without collision
"""
backtracking = False
while True:
x, y = self._path.next()
translated_x, translated_y = \
utils.translate_coordinates_between_systems(x, y,
robot_x, robot_y, robot_angle)
if not self._laser.is_observable(translated_x,translated_y):
try:
path_x
except NameError:
backtracking=True
break
path_x,path_y = translated_x,translated_y
if backtracking:
while True:
x, y = self._path.previous()
translated_x, translated_y = \
utils.translate_coordinates_between_systems(x, y,
robot_x, robot_y, robot_angle)
if self._laser.is_observable(translated_x, translated_y):
path_x, path_y = translated_x, translated_y
break
try:
while True:
if self._laser.check_if_circle_safe(path_x,path_y):
return path_x, path_y
else:
try:
x, y = self._path.previous()
path_x, path_y = \
utils.translate_coordinates_between_systems(x, y,
robot_x, robot_y, robot_angle)
except EndOfPathError:
raise NoPointObservableError()
except NameError:
raise NoPointObservableError()
def mainGame(movementInfo):
global playerAccX
global playerAccY
#define publishers
pub_velocity = rospy.Publisher('flappy_vel', Vector3, queue_size=10)
# create laser
laser = Laser(LASERFOV,LASERRES,SCALING)
score = playerIndex = loopIter = 0
playerIndexGen = movementInfo['playerIndexGen']
playerx, playery = int(SCREENWIDTH * 0.13), movementInfo['playery']
#create timer and counter for timer countdown
pygame.time.set_timer(pygame.USEREVENT, 1000)
countdown = 60
#
basex = movementInfo['basex']
baseShift = IMAGES['base'].get_width() - IMAGES['background'].get_width()
# get 2 new pipes to add to upperPipes lowerPipes list
newPipe1 = getRandomPipe()
newPipe2 = getRandomPipe()
# list of upper pipes
upperPipes = [
{'x': SCREENWIDTH + 200, 'y': newPipe1[0]['y']},
{'x': SCREENWIDTH + 200 + PIPESPACING, 'y': newPipe2[0]['y']},
]
# list of lowerpipe
lowerPipes = [
{'x': SCREENWIDTH + 200, 'y': newPipe1[1]['y']},
{'x': SCREENWIDTH + 200 + PIPESPACING, 'y': newPipe2[1]['y']},
]
# player velocity, max velocity, downward accleration, accleration on flap
pipeVelX = 0
playerVelY = 0 # player's velocity along Y
deltaVel = 0.8
betweenPipes = 0
while True:
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
pygame.quit()
sys.exit()
if event.type == KEYDOWN and (event.key == K_UP):
#if playery > -2 * IMAGES['player'][0].get_height():
playerVelY -= deltaVel
#playerAccY -= deltaAcc
if event.type == KEYDOWN and (event.key == K_DOWN):
playerVelY += deltaVel
#playerAccY += deltaAcc
if event.type == KEYDOWN and (event.key == K_LEFT):
pipeVelX += deltaVel
#playerAccX += deltaAcc
if event.type == KEYDOWN and (event.key == K_RIGHT):
pipeVelX -= deltaVel
#playerAccX -= deltaAcc
if event.type == pygame.USEREVENT and score > 0:
if countdown > 0:
countdown -= 1
else:
return {
'y': playery,
'groundCrash': crashTest[1],
'basex': basex,
'upperPipes': upperPipes,
'lowerPipes': lowerPipes,
'score': score,
'playerVelY': playerVelY,
'timeRanOut': 1,
}
#update velocity
pipeVelX += playerAccX
playerVelY += playerAccY
#limit velocity
playerVelY = limitVel(playerVelY,1)
pipeVelX = limitVel(pipeVelX,0)
#publish pub_velocity
pub_velocity.publish(Vector3(-SCALING*FPS*pipeVelX,-SCALING*FPS*playerVelY,0))
# check for crash here
crashTest = checkCrash({'x': playerx, 'y': playery, 'index': playerIndex},
upperPipes, lowerPipes)
if crashTest[0]:
return {
'y': playery,
'groundCrash': crashTest[1],
'basex': basex,
'upperPipes': upperPipes,
'lowerPipes': lowerPipes,
'score': score,
'playerVelY': playerVelY,
'timeRanOut': 0,
}
# check for score
playerMidPos = playerx + IMAGES['player'][0].get_width() / 2
pipeCounter = 0
for pipe in upperPipes:
pipeMidPos = pipe['x'] + IMAGES['pipe'][0].get_width() / 2
if pipeMidPos <= playerMidPos < (pipeMidPos + IMAGES['pipe'][0].get_width() / 2):
pipeCounter += 1
if betweenPipes == 0:
score += 1
betweenPipes = 1
SOUNDS['point'].play()
if pipeCounter == 0:
betweenPipes = 0
# playerIndex basex change
if (loopIter + 1) % 3 == 0:
playerIndex = next(playerIndexGen)
loopIter = (loopIter + 1) % 30
basex = -((-basex - pipeVelX) % baseShift)
playerHeight = IMAGES['player'][playerIndex].get_height()
playery += min(playerVelY, BASEY - playery - playerHeight)
# move pipes to left
for uPipe, lPipe in zip(upperPipes, lowerPipes):
uPipe['x'] += pipeVelX
lPipe['x'] += pipeVelX
# add new pipe when first pipe each ? pixels
if (SCREENWIDTH + 200) > upperPipes[-1]['x']:
newPipe = getRandomPipe()
newPipe[0]['x'] = PIPESPACING + upperPipes[-1]['x']
newPipe[1]['x'] = PIPESPACING + upperPipes[-1]['x']
upperPipes.append(newPipe[0])
lowerPipes.append(newPipe[1])
# remove first pipe if its out of the screen
if upperPipes[0]['x'] < -IMAGES['pipe'][0].get_width():
upperPipes.pop(0)
lowerPipes.pop(0)
# draw sprites
SCREEN.blit(IMAGES['background'], (0,0))
for uPipe, lPipe in zip(upperPipes, lowerPipes):
SCREEN.blit(IMAGES['pipe'][0], (uPipe['x'], uPipe['y']))
SCREEN.blit(IMAGES['pipe'][1], (lPipe['x'], lPipe['y']))
SCREEN.blit(IMAGES['base'], (basex, BASEY))
###################################################################
# get bitmap of obstacles
bitmap = getBitmap(upperPipes,lowerPipes,(basex, BASEY))
# do raytracing with Laser
playerMiddle = (playerx + IMAGES['player'][0].get_width() / 2,playery + IMAGES['player'][0].get_height() / 2)
laserPoints = laser.scan(playerMiddle,bitmap)
# display
if DEBUG == 1:
# display obstacles and ray tracing
bitmap = pygame.surfarray.make_surface(bitmap)
SCREEN.blit(bitmap, (0, 0))
for i in range(0,len(laserPoints)):
if laserPoints[i][2] == 1:
pygame.draw.circle(SCREEN,(0,255,0),laserPoints[i][0:2],3,0)
pygame.draw.aaline(SCREEN,(0,255,0),playerMiddle,laserPoints[i][0:2],1)
else :
pygame.draw.aaline(SCREEN,(0,140,0),playerMiddle,laserPoints[i][0:2],1)
###################################################################
# print score so player overlaps the score
showScore(score)
if score > 0:
showCounter(countdown)
playerSurface = pygame.transform.rotate(IMAGES['player'][playerIndex], 0)
SCREEN.blit(playerSurface, (playerx, playery))
pygame.display.update()
FPSCLOCK.tick(FPS)
if right_or_left == "right":
obstacle.go_right()
elif right_or_left == "left":
obstacle.go_left()
if right_or_left == "right":
right_or_left = "left"
elif right_or_left == "left":
right_or_left = "right"
if random_num == 20 or random_num == 30:
laser_move_speed += 0.1
random_enemy = random.choice(obstacles)
x = random_enemy.xcor()
y = random_enemy.ycor()
laser = Laser((x, y), "Down")
lasers.append(laser)
for laser in lasers:
laser.move_laser(laser_move_speed)
for laser in lasers:
if laser.distance(player) < 20 and laser.direction == "Down":
life -= 1
laser.goto(10000, 10000)
index = lasers.index(laser)
lasers.pop(index)
for obstacle in obstacles:
if laser.distance(obstacle) < 20 and laser.direction == "Up":
laser.goto(10000, 10000)
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
from time import sleep
from panandtilt import Panandtilt
from laser import Laser
head = Panandtilt(5, 150, 6, 150)
head.servo_reset_position
point = Laser(8)
point.on()
for x in range(0, 100):
print head.pan_current_position
print head.tilt_current_position
head.pan_left(1)
sleep(0.01)
for x in range(0, 100):
print head.pan_current_position
print head.tilt_current_position
head.pan_right(1)
sleep(0.01)
for x in range(0, 100):
class LowControl:
""" Lowcontrol represents a planner for the tracker robot over on MRDS
Attributes:
linear the current linear speed
angular the current angular speed
mrds a mrdsapi object that handles api requests
laser a laser object that handles laser data
"""
def __init__(self, mrds):
self.linear = None
self.angular = None
self.mrds = mrds
self.laser = Laser(mrds)
return
def set_linear_speed(self, linear):
"""
Updates both the planner and tracker linear speed, angular speed
remains unchanged
"""
self.linear = linear
self.mrds.post_speed(self.angular, linear)
def set_angular_speed(self, angular):
"""
Updates both the planner and tracker angular speed, linear speed
remains unchanged
"""
self.angular = angular
self.mrds.post_speed(angular, self.linear)
def set_speed(self, angular, linear):
"""Updates both angular and linear speed for both planner and tracker"""
self.linear = linear
self.angular = angular
self.mrds.post_speed(angular, linear)
def steer_to_point(self, loc, gp, speed):
"""
Receives speed and curvature and tells the robot to start
the drive to a point
"""
L = utils.pos_dist(loc, gp)
# Construct a circle passing through (0,0)RCS and GP, such that the
# vehicle orientation is a tangent to the circle The circle is defined
# by its radius r and midpoint We know that r = L^2/2y from geometry
y = utils.rcs_y_dist(loc, gp)
r = (L ** 2) / (2 * y)
# Set phi or omega to correspond to motion along this circle, omega = vY
# where v is the linear speed and Y=1/r is the curvature of the circle
Y = 1 / r
s = speed
# sleep briefly to prevent socket overload
# omega = vY according to lecture notes
omega = s*Y
crash = self.laser.will_crash()
# Is about to crash
if crash:
s = 0
if utils.angle_dist(loc, gp) > math.pi:
s = .2
omega *= 100
self.set_speed(omega, s)
def get_location(self):
"""Returns a pose object for the robots current position"""
return self.mrds.get_localization()
def get_laser_scan(self):
"""Performs a laser scan on the robot and returns the result"""
return self.mrds.get_laser_echoes()
def reached_point(self, gp):
"""
Uses the rules from the specification to determine if the
robot has reached the specified point
"""
p1 = self.mrds.get_localization()
return utils.pos_dist(p1, gp) < .4
def stop_robot(self):
"""Stops the robot"""
self.set_speed(0, 0)
# -*- coding: utf-8 -*-
"""
Created on Wed May 22 15:40:53 2019
@author: Kyle DeBry
"""
from pure_photonics_utils import *
from laser import Laser
import curses
import logging
import sys
laser = Laser('COM2', 9600, logging.DEBUG)
try:
freq = 193
laser_err = laser.laser_on(freq)
print('Laser error: %d' % laser_err)
time.sleep(1)
if laser_err == ITLA.NOERROR:
sled_slope = laser.get_sled_slope()
sled_spacing = laser.get_sled_spacing(
'CalibrationFiles\\CRTNHBM047_21_14_43_4.sled')
map_vals = Laser.read_mapfile(
# -*- coding: utf-8 -*-
"""
Created on Wed May 22 15:40:53 2019
@author: Kyle DeBry
"""
from pure_photonics_utils import *
from laser import Laser
import logging
laser = Laser('COM2', 9600, log_level=logging.DEBUG)
laser.laser_off()
try:
print(laser)
print(laser.sercon)
test_response = laser.itla_communicate(ITLA.REG_Nop, 0, ITLA.READ)
print(test_response)
if test_response == ITLA.ERROR_SERBAUD:
print('Baud rate error')
elif test_response == ITLA.ERROR_SERPORT:
print('Port connection error')
else:
print('Connection successful! :)')
sled_slope = laser.get_sled_slope()
file_index = int(sys.argv[3])
if len(sys.argv) > 4:
suppress_robot_comm = True
else:
suppress_robot_comm = False
while 1:
if lp is None:
# open up the serial port device connected to the lidar
try:
#lp = serial.Serial('/dev/ttyUSB0',115200,timeout=1)
#lp = serial.Serial('/dev/tty.usbserial',115200,timeout=1)
#lp = serial.Serial('/dev/tty.wchusbserial1420',115200,timeout=1)
lp = serial.Serial(serial_port_name, 115200, timeout=1)
lasr = Laser(lp)
except:
logger.critical('Unable to open lidar port: {}'.format(serial_port_name))
logger.critical('Try /dev/ttyUSB0, or maybe /dev/tty.wchusbserial1420, or maybe /dev/tty.usbserial')
periodic_message.status = 'down'
periodic_message.rpm = 0
if not suppress_robot_comm:
channel.send_to(periodic_message.encode_message())
logger.error('Lidar port could not be opened.')
if lasr is not None:
try:
# NOTE: because lidar is upside down, this needs to be reversed?
rotation = Rotation(lasr.gather_full_rotation(reverse_data=True))
#
def __init__(self, window):
"""Initializes an instance of the Game class.
:param window: the pygame support module window object
"""
# Static class attributes
self.window = window
self.surface = window.get_surface()
self.close_clicked = False
self.continue_game = True
self.pressed = None
self.ship_exploded = False
self.laser_list = [] # A list to contain all Laser Objects
self.asteroid_list = [] # A list to contain all Asteroid Objects
self.star_list = [] # A list to contain all Star Objects
self.last_fire = 0
self.clock = 0
self.game_end_clock = 0
self.last_spawn = 0
self.score = 0
# Adjustable class attributes
self.header_1_size = 50
self.header_2_size = 20
self.left_margin = 100
self.ship_size = (50, 50)
self.ship_lateral_speed = 8
self.ship_height = 615 # The fixed distance from the top of the window where the ship will be
self.asteroid_buffer = 20 # Initial buffer only
self.asteroid_buffer_decrease = 0.05
self.asteroid_speed = 2 # Initial speed only
self.asteroid_speed_increase = 0.05
self.asteroid_size = 50
self.asteroid_margin_x = 200 # The lateral distance beside the window in which an asteroid may spawn
self.asteroid_margin_y = 100 # The vertical distance above the window in which an asteroid may spawn
self.laser_buffer = 15
self.laser_buffer_intro = 20 # Adjust this to change the amount of lasers that fire before the game begins
self.laser_size = (30, 50)
self.laser_offset = 15 # May be adjusted to center the laser over the ship when firing
self.laser_speed = 20
self.star_population_size = 500 # The amount of stars that should initially populate the screen
self.star_size = 2
self.star_velocity = (0, 1)
self.pause_time = 0.02 # Smaller number is faster game
# Create image/Rect objects for space ship animation
self.ship_img_1 = pygame.transform.scale(pygame.image.load("images/ship_1.png"), self.ship_size)
self.ship_img_2 = pygame.transform.scale(pygame.image.load("images/ship_2.png"), self.ship_size)
self.explosion_1 = pygame.transform.scale(pygame.image.load("images/explosion_1.png"), self.ship_size)
self.explosion_2 = pygame.transform.scale(pygame.image.load("images/explosion_2.png"), self.ship_size)
self.explosion_3 = pygame.transform.scale(pygame.image.load("images/explosion_3.png"), self.ship_size)
self.explosion_4 = pygame.transform.scale(pygame.image.load("images/explosion_4.png"), self.ship_size)
self.explosion_5 = pygame.transform.scale(pygame.image.load("images/explosion_5.png"), self.ship_size)
self.explosion_6 = pygame.transform.scale(pygame.image.load("images/explosion_6.png"), self.ship_size)
self.ship_rect = pygame.Rect(self.window.get_width()/2, self.ship_height, self.ship_size[0], self.ship_size[1])
# Set the window for Star, Laser, and Asteroid Objects
Star.set_window(self.window)
Laser.set_window(self.window)
Asteroid.set_window(self.window)
def shoot(self):
laser = Laser(int(self.x + self.ship_img.get_width() / 2 - 130),
self.y + self.ship_img.get_height() - 80, self.laser_img)
self.lasers.append(laser)
def __init__(self, path, communicator):
self._path = path
self._laser = Laser(communicator)
def shoot(self):
self.laser_list.append(
Laser(self.rect.centerx, self.rect.top, -7, self))
laserSound.play()
from laser import Laser laser = Laser() laser.off()
lidar_logger = LidarLogger(logger)
lidar_viewer = LidarViewer()
lp = None
# open up the serial port device connected to the lidar
try:
# lp = serial.Serial('/dev/ttyUSB0',115200,timeout=1)
lp = serial.Serial("/dev/tty.usbserial", 115200, timeout=1)
# lp = serial.Serial('/dev/tty.wchusbserial1420',115200,timeout=1)
except:
logger.error("Lidar port could not be opened.")
# connect the port with the laser and initialize the laser object
# lasr = OldLaser(lp)
lasr = Laser(lp)
#
#
slice_index = 0
file_index = 1
rotation_time = 0
current_time = 0
seconds_per_output = 1
SECONDS_PER_MINUTE = 60.0
rotations = []
while 1:
try:
rotation = Rotation(lasr.gather_full_rotation())
rotations = rotations[-4:]
rotations.append(rotation)
def laserThreadFunc():
log.debug("laserThreadFunc 线程启动 ...")
if SWITCH_DEVICE:
LASER3_COM = "com3"
else:
LASER3_COM = "com20"
LASER_REC_BUF_LEN = 11
laser3 = Laser(LASER3_COM)
time.sleep(0.01)
laser_min_distance = 0.1 # 激光安装最小距离
laserLed = False
""" 等待数据准备 """
while True:
laser_rec_buf = laser3.laser_read_data(LASER_REC_BUF_LEN)
if laser_rec_buf is not None:
# 校验
check_out = laserCheck(laser_rec_buf)
last_val = int.from_bytes(laser_rec_buf[-1:],
byteorder='little',
signed=False)
if check_out == last_val:
laser3_dist = laser3.get_distance(laser_rec_buf)
if laser3_dist > laser_min_distance:
laser_data_valid_flg = True
# 设置全局变量
my_lock.pSor_laser_main_lock.acquire()
gl.set_value("dou_laser_len", laser3_dist)
gl.set_value("laser_data_valid_flg", laser_data_valid_flg)
my_lock.pSor_laser_main_lock.release()
break
else:
print(
"LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLdou_laser_len:%s\n"
% laser3_dist)
else:
print("!!!!!!!激光校验失败!!!!!!\n")
time.sleep(0.05)
""" 正常流程 """
while True:
if laser3.com_laser.com.is_open:
laser_rec_buf = laser3.laser_read_data(LASER_REC_BUF_LEN)
if laser_rec_buf is not None:
# 校验
check_out = laserCheck(laser_rec_buf)
last_val = int.from_bytes(laser_rec_buf[-1:],
byteorder='little',
signed=False)
if check_out == last_val:
laser3_dist = laser3.get_distance(laser_rec_buf)
if laser3_dist > laser_min_distance:
# print("=======================挖斗激光%f" % laser3_dist)
dou_laser_len = laser3_dist
laserLed = True
# 设置全局变量
my_lock.pSor_laser_main_lock.acquire()
gl.set_value("dou_laser_len", dou_laser_len)
my_lock.pSor_laser_main_lock.release()
else:
laserLed = False
print("激光距离错误\n")
else:
laserLed = False
print("！！！！！ 激光校验失败 ！！！！\n")
else:
laserLed = False
# 设置全局变量
my_lock.laserLedLock.acquire()
gl.set_value("laserLed", laserLed)
my_lock.laserLedLock.release()
time.sleep(0.15)
offset_x = 0.0
offset_y = 0.0
resolution = 0.1
# Create map and laser scans
occ_map = Map.readFromTXT('../map.txt', width, height, offset_x, offset_y,
resolution)
max_range = 50.0
no_of_beams = 181
min_angle = -math.pi / 2.0
resolution_angle = math.pi / (no_of_beams - 1)
noise_variance = 0.0
laser_pos_x = 1.2
laser_pos_y = 0.0
laser_angle = 0.0 * (math.pi / 180.0)
laser = Laser(max_range, min_angle, resolution_angle, no_of_beams,
noise_variance, occ_map, laser_pos_x, laser_pos_y, laser_angle)
# Read positions
positions = np.loadtxt('../data_pose.txt')
# Read corresponding laser scans
#laser_scans = np.loadtxt('../map_scans.txt') #, delimiter=','
# Go through each position and generate a new scan
# Validate each generated scan against recorded ones
counter = 0
all_ranges = []
n = 2000
for i in range(n):
ranges = laser.scan(positions[i, 0], positions[i, 1], positions[i, 2])
all_ranges.append(copy(ranges))
# ------ moves Paco on a map ------
paconator.move()
paconator.direction.x = 0
paconator.direction.y = 0
# ------ prints Paco ------
for y in range(5):
for x in range(15):
map.set_point(paconator.sprite[y][x].x,
paconator.sprite[y][x].y,
paconator.sprite[y][x].character)
# ====== LASERS ======
# ------ shoots laser if 'space' pressed ------
if key == 32 and ammo > 0:
paconator.sprite[1][8].character = 'X'
laser = Laser(paconator.sprite[1][6].x, paconator.sprite[1][6].y,
133, 33)
lasers.append(laser)
ammo -= 1
else:
paconator.sprite[1][8].character = '@'
for index in range(len(lasers)):
lasers[index].direction.x = 3
lasers[index].move()
for index in range(len(lasers)):
for x in range(5):
map.set_point(lasers[index].sprite[0][x].x,
lasers[index].sprite[0][x].y,
lasers[index].sprite[0][x].character)
# ------ destroys laser if it hits map border ------
for index in range(len(lasers)):
if lasers[index].sprite[0][4].x == 131:
def shoot():
time.sleep(move_speed)
new_laser = Laser((player.xcor(), player.ycor()), "Up")
lasers.append(new_laser)
def shoot(self, x, y):
if self.shoot_cooldown == 0:
shoot_sound.play()
self.shoot_cooldown = 45
laser = Laser(self.screen, self.laser_img, x, y, self.laser_speed)
self.lasers.append(laser)
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
from time import sleep
from panandtilt import Panandtilt
from laser import Laser
head = Panandtilt(5, 150, 6, 150)
head.servo_reset_position
point = Laser (8)
point.on()
for x in range(0,100):
print head.pan_current_position
print head.tilt_current_position
head.pan_left(1)
sleep(0.01)
for x in range(0,100):
print head.pan_current_position
print head.tilt_current_position
head.pan_right(1)
sleep(0.01)
for x in range(0,100):
class Square:
def __init__(self):
print "Set mode BCM"
config = json.load(open("config.json"))
GPIO.setmode(GPIO.BCM)
self.motorX = Motor(config["motor_x"]["pins"])
self.motorX.name = "X"
self.motorX.delay = config["motor_x"]["delay"]
self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]
self.motorY = Motor(config["motor_y"]["pins"])
self.motorY.name = "Y"
self.motorY.delay = config["motor_y"]["delay"]
self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]
self.laser = Laser(config["laser"]["pin"])
def start(self):
size = 10
self.laser.on()
self.move(self.motorY, Motor.RIGHT, size)
self.move(self.motorX, Motor.RIGHT, size)
self.move(self.motorY, Motor.LEFT, size)
self.move(self.motorX, Motor.LEFT, size)
self.laser.off()
self.motorX.off()
self.motorY.off()
def move(self, motor, direction, size):
print "motor %s %d %d" % (motor.name, direction,
size * motor.steps_by_mm)
for x in range(0, int(size * motor.steps_by_mm)):
motor.moveTo(direction)
def cleanUp(self):
GPIO.cleanup()
