def parse(command):
commands = command.split(
" ") # Split "command key key2" into ["command", "key", "key2"]
print("Command: " + str(commands))
if (commands[0].lower() == "connect"):
if (len(commands) <
2): # If no ip-address is given connect to default address
commands.append("0")
CommunicationBase.StartProcess(commands[1])
elif (commands[0].lower() == "other"):
globals.SendOther("other", commands)
elif (commands[0].lower() == "ping"
): # Changes the time between robot and base communication
globals.sharedData["LocalPing"] = float(commands[1])
globals.SendCommand(
commands) # Send change to robot so it can update also
elif (commands[0].lower() == "joystick"
): # Start and stop the joystick process
if (commands[1].lower() == "on"):
Joystick.StartJoystickProcess()
elif (commands[1].lower() == "off"):
Joystick.StopJoystickProcess()
else:
globals.SendCommand(
commands
) # If its not in the list, send it to the robot for processing
def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC,
predefined, h_init, kf_enabled, perfectEstimator):
"""Create several objects that are used in the control loop
Args:
dt_tsid (float): time step of TSID
dt_mpc (float): time step of the MPC
k_max_loop (int): maximum number of iterations of the simulation
k_mpc (int): number of tsid iterations for one iteration of the mpc
T_mpc (float): duration of mpc prediction horizon
type_MPC (bool): which MPC you want to use (PA's or Thomas')
predefined (bool): if we are using a predefined reference velocity (True) or a joystick (False)
h_init (float): initial height of the robot base
kf_enabled (bool): complementary filter (False) or kalman filter (True)
perfectEstimator (bool): if we use a perfect estimator
"""
# Create Joystick object
joystick = Joystick.Joystick(predefined)
# Create logger object
logger = Logger.Logger(k_max_loop, dt_tsid, dt_mpc, k_mpc, T_mpc, type_MPC)
# Create Estimator object
estimator = Estimator.Estimator(dt_tsid, k_max_loop, h_init, kf_enabled,
perfectEstimator)
return joystick, logger, estimator
def Input():
global joydir
global lose
i = getdir(Joy.location())
if i != 0:
joydir = i
if lose == False: root.after(10, Input)
def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, n_periods):
# Create Joystick object
joystick = Joystick.Joystick(k_mpc)
# Create footstep planner object
fstep_planner = FootstepPlanner.FootstepPlanner(dt_mpc, n_periods)
# Create logger object
logger = Logger.Logger(k_max_loop, dt_tsid, dt_mpc, k_mpc, n_periods)
# Create Interface object
interface = Interface.Interface()
return joystick, fstep_planner, logger, interface
def createJoystick(self, id):
return Joystick(self, id)
from ..rov-topside2015 import Joystick joy = Joystick() joy.list_joysticks()
""" Snake Game on a 8x8 LED Matrix made by Gladox114 https://pastebin.com/v0wVd9Sp """ import Matrix as M import Joystick as Joy #import Display as disp import random import time import threading import copy border=True Map = 1,1,8,8 Joy.setup() M.setup() display=[ "00000000", "00000000", "00010000", "00101000", "00010100", "00001010", "00000100", "00000000"] u=[ "00000000", "00000000",
def MoveAvatarRandomly(avatar, moveProbability=None, speed=None):
global layoutScale
if not moveProbability:
if avatar.moveProbability:
moveProbability = avatar.moveProbability
if avatar.backupMoveProbability:
avatar.moveProbability = avatar.backupMoveProbability
avatar.backupMoveProbability = None
else:
avatar.moveProbability = None
else:
moveProbability = DEFAULT_MOVE_PROBABILITY
# If the avatar's speed was not specified,
# randomly choose a speed.
if not speed:
speed = WALK_SCALE * random.uniform(0.9, 1.1)
if avatar.interrupted == "Delay":
avatar.interrupted = "Continue"
elif avatar.interrupted == "Continue":
MoveAvatar(avatar, avatar.destination, speed)
avatar.interrupted = "No"
return
(intersCenter, rotation) = __state2viz(avatar.destination)
# Build the list of valid directions.
if moveProbability[4] > 0:
validDirections = [None]
else:
validDirections = []
for direction in range(0, 360, 90):
testState = (intersCenter[0], intersCenter[2], direction)
# Take a measurement above the heads of the other avatars.
length = Joystick.getHallLength(testState, layoutScale, 0.3)
if length > 0:
validDirections.append(direction)
# Randomly pick the next movement for this avatar based on moveProbability.
oldDirection = avatar.destination[2]
directionProbs = {}
if oldDirection in validDirections:
directionProbs[oldDirection] = moveProbability[0]
oldLeft = (oldDirection - 90) % 360
if oldLeft in validDirections:
directionProbs[oldLeft] = moveProbability[1]
oldRight = (oldDirection + 90) % 360
if oldRight in validDirections:
directionProbs[oldRight] = moveProbability[2]
oldBack = (oldDirection + 180) % 360
if oldBack in validDirections:
directionProbs[oldBack] = moveProbability[3]
if None in validDirections:
directionProbs[None] = moveProbability[4]
normalize = sum(directionProbs.values())
randomNum = random.random() * normalize
for (dir, prob) in directionProbs.items():
if randomNum < prob:
newDirection = dir
break
else:
randomNum -= prob
else:
raise "Error choosing random direction."
# Start the Avatar's next move.
if newDirection == None:
LoiterAvatar(avatar)
else:
x = avatar.destination[0]
y = avatar.destination[1]
if newDirection == 0:
y -= 1
elif newDirection == 90:
x += 1
elif newDirection == 180:
y += 1
elif newDirection == 270:
x -= 1
avatar.destination = (x, y, newDirection)
offset = (random.uniform(-0.075, 0.075), random.uniform(-0.075, 0.075))
destination = (x + offset[0], y + offset[1], newDirection)
MoveAvatar(avatar, destination, speed)
from StageSPI import *
from StageI2C import *
from time import *
import pygame
# Clear the Linux built-in joystick deadzone
exec(open("Deadzone.py").read())
# Initialize all 3 stages
x = StageSPI(0, 0)
y = StageSPI(0, 1)
z = StageI2C(1, 0x40)
# Initialize pygame and joystick functions
pygame.init()
joystick = Joystick()
buttons, axes = joystick.get_all()
# Declare default home position and current position
home = [DefaultHome, DefaultHome, DefaultHome]
position = [DefaultHome, DefaultHome, DefaultHome]
# Declare default sensitivity level for x and y axes
sensitivity_level = 0
# Declare sensitivity level for z axis
sensitivity_Z_High = True
Z_Sensitivity = Z_Sensitivity_High
# Declare if both x and y axes has been homed
# Threshold for homing declared in config.py
def __init__(self,
prefix,
env,
numRuns=0,
exptName="",
mapDir="Maps/",
onFinish=None,
init_mode='Explore',
group=0,
positionList=[],
numMoves=100,
efficiency=0.65,
directionDir="Directions/",
movie=None,
movement='Discrete'):
self.mode = init_mode
self.prefix = prefix
self.layout = exptName + env
self.exptName = exptName
self.outfile = prefix + "_" + self.layout + ` group ` + "_"
self.mapdir = mapDir
self.logdir = 'SubjectLogs/'
self.directionDir = directionDir
matches = getFiles(self.outfile, self.logdir)
self.outfile += ` len(matches) + 1 ` + ".out"
self.currVersion = "Train $Revision: 1.86 $"
print "Using layout:", (self.layout + ".wrl")
print "Using outfile:", self.outfile
self.numGoodTrials = numRuns
self.numSuccess = 0
self.numFailures = 0
self.numOK = 0
self.onFinish = onFinish
self.group = group
self.positionList = positionList
self.directions = None
self.moveBound = numMoves
self.jump = 0
self.group = group
self.trialAccuracy = efficiency
self.movie = movie
if movement == 'Discrete': self.movement = Movement
elif movement == 'Continuous':
self.movement = Joystick.ContinuousMovement(
scale=SCALE,
center_offset=SCALE / 10,
keyboard=True,
joyCalFile='NavigationModules/joyCal.dat',
callback=self.fwdMovementCallback,
backward=False,
speed=7.5)
else:
raise ValueError, 'Unknown movement type requested: ', movement
if (self.mode == 'Directions'): HotSpots.SoundOn(0)
self.cues = VisualCueList()
self.envs = {}
self.envCues = {}
self.VisitedHotSpots = HotSpotList()
if env == 'All':
self.loadAllEnvs()
env = 'Grid'
else:
print 'Loading env'
self.room = viz.addchild(self.mapdir + self.layout + ".wrl")
print 'Setting appearance'
self.room.appearance(viz.DECAL)
self.envs[self.layout] = self.room
print 'Loading cues'
self.cues.readInCues(self.mapdir + self.layout + ".obj")
self.envCues[self.layout] = self.cues.CueList
print 'Switching to env'
self.switchLayout(env, self.group)
for cues in self.envCues.values():
for cue in cues:
cue.Object.enable(viz.LIGHTING)
cue.Object.disable(viz.COLLISION)
self.outputHeader()
self.starttime = time.time()
self.closeCurtain()
SoundQueue.RegisterWithMovementModule(self.movement)
import Joystick joystick = Joystick() joy.list_joysticks()
def __init__(self):
global ARM_ADDRESS, SENSOR_ADDRESS, DRIVE_ADDRESS, SENSOR_READ_ADDRESSES
global SENSORS, ser, pser
QMainWindow.__init__(self)
self.setAttribute(Qt.WA_DeleteOnClose)
self.setWindowTitle("application main window")
self.resize(1000, 1000)
# --- Menu --- #
self.file_menu = QMenu('&File', self)
self.file_menu.addAction('&Quit', self.fileQuit, Qt.CTRL + Qt.Key_Q)
self.menuBar().addMenu(self.file_menu)
# --- Widgets --- #
self.tabs = QTabWidget()
self.graph_widget = QWidget()
self.map_widget = QWidget()
self.topview_widget = QWidget()
# Combo box for selecting which graphs to view
self.graph_chooser_top = QComboBox(self.graph_widget)
self.graph_chooser_top.addItems(['Gas Sensor', 'Moisture Sensor'])
self.graph_chooser_bottom = QComboBox(self.graph_widget)
self.graph_chooser_bottom.addItems(['Gas Sensor', 'Moisture Sensor'])
# Dynamic canvases to hold graphs
dc1 = Graphs.dynamic_graph_canvas(Graphs.dynamic_graph_canvas.\
gas_sensor, parent=self.graph_widget, width=5, height=4, dpi=100)
dc2 = Graphs.dynamic_graph_canvas(Graphs.dynamic_graph_canvas.\
moisture_sensor, parent=self.graph_widget, width=5, height=4, dpi=100)
# Make map widget
if GPSGui.marble_imported: # see import statement at top for reason
UTIAS = Marble.GeoDataCoordinates(-79.466083, 43.782247, 0,
Marble.GeoDataCoordinates.Degree)
gps_map = GPSGui.GPSMapWidget(UTIAS, 'UTIASTest.osm',
'/dev/ttyUSB0', '/dev/ttyUSB1')
# Start joystick
try: # so that it can still run without joystick connected
self.joystick_thread = QThread()
self.j = Joystick.Joystick(ARM_ADDRESS, DRIVE_ADDRESS, ser, pser)
self.j.moveToThread(self.joystick_thread)
self.connect(self.joystick_thread, SIGNAL("started()"),
self.j.start_joystick)
self.connect(self.joystick_thread, SIGNAL("finished()"),
self.j.end_joystick)
self.joystick_thread.start()
except:
print "Failed to start joystick: joystick probably not plugged in"
# start sensor getter
try:
self.sensor_getter_thread = QThread()
self.sg = SensorGetter.SensorGetter(SENSOR_READ_ADDRESSES, SENSORS,
ser, pser)
sef.sg.moveToThread(self.sensor_getter_thread)
self.connect(self.sensor_getter_thread, SIGNAL("started()"),
self.j.start_sg)
self.connect(self.sensor_getter_thread, SIGNAL("finished()"),
self.j.end_sg)
self.sensor_getter_thread.start()
except:
print "Failed to start sensor getter"
# Coloured wheels widget
rov = RoverTopview.RoverTopview()
# Box layout for graph_widget, holds 2 graph canvases and combo box
l = QHBoxLayout()
v1 = QVBoxLayout()
v2 = QVBoxLayout()
v1.addWidget(dc1)
v1.addWidget(self.graph_chooser_top)
v2.addWidget(dc2)
v2.addWidget(self.graph_chooser_bottom)
l.addLayout(v1)
l.addLayout(v2)
self.graph_widget.setLayout(l)
# Horizontal box layout for map_widget, holds label with map image
l2 = QHBoxLayout()
if GPSGui.marble_imported: # see import for reason
l2.addWidget(gps_map)
self.map_widget.setLayout(l2)
# Layout for topview_widget
l3 = QHBoxLayout()
l3.addWidget(rov)
self.topview_widget.setLayout(l3)
# Combo box event
self.graph_chooser_top.activated[str].connect(dc1.graph_change_handler)
self.graph_chooser_bottom.activated[str].connect(
dc2.graph_change_handler)
# Create tabs
self.tabs.addTab(self.graph_widget, 'Graphs')
self.tabs.addTab(self.map_widget, 'Map')
self.tabs.addTab(self.topview_widget, 'Top View')
self.tabs.setWindowTitle('RSX Rover Control')
self.tabs.show()
if len(snake) > 4:
for i in range(0, len(snake) - 1):
if snake[-1] == snake[i]:
c.create_oval(pos(snake[i][0], snake[i][1]),
fill="red",
outline="red",
tags="snake")
return True
else:
return False
#---------------------- Key Controlls ---------------------
if True: # Because then I can hide the Controlls
import Joystick as Joy
Joy.setup()
border = False
joydir = "right"
lastdirection = "right"
MapChunks = 0, 0, int(Map / Block - 1), int(Map / Block - 1)
def getdir(x, *y):
if type(x) == tuple:
x, y = x
if y == 1:
y = "down"
elif y == 2:
y = "up"
if x == 1:
x = "left"
elif x == 2:
#!/usr/bin/env python
#coding=utf-8
import rospy
import Joystick
if __name__ == "__main__":
rospy.init_node("joystick_control_node", anonymous=True)
rate = rospy.Rate(100)
#Start joystick publisher
joystick_publisher = Joystick.Joystick()
rospy.loginfo("Joystick_publisher_node started")
while (not rospy.is_shutdown()):
rate.sleep()
#键盘被放开
if event.type == KEYUP:
if event.key == K_RETURN:
game.goto_next_level()
#监测键盘
keys = pygame.key.get_pressed()
#退出游戏
if keys[K_ESCAPE]:
sys.exit()
#游戏未结束
if not cfg.game_over:
# Joystick Controller
Joystick.setup()
joystick = Joystick.direction()
#游戏相关操作与检查碰撞
game.update_blocks(ticks)
game.move_paddle(joystick,ticks)
game.move_ball(ticks)
game.collision_ball_paddle()
game.collision_ball_blocks()
#用蓝色填充窗口
game.screen.fill((50,50,100))
#绘制精灵组
game.block_group.draw(game.screen)
game.ball_group.draw(game.screen)
def main(self):
pygame.init()
pygame.display.set_caption("SWIV for Pi")
#pygame.mixer.init()
bg1 = pygame.image.load("images/bg1.gif")
title = pygame.image.load("images/title.gif")
tx1 = pygame.image.load("images/text1.gif")
#bgm = "sfx/bmlevel1.mid"
#pygame.mixer.music.load(bgm)
clock = pygame.time.Clock()
size = width, height = 200, 300
screen = pygame.display.set_mode(size)
myfont = pygame.font.SysFont("monospace", 11)
timer = 0
debug = 0
score = 0
buttonPressed = 0
gameOver = 1
deathCounter = 0
player = Player.Player(80, -100) # First call module, then the class
copterBlades = player.getSpriteBlade().get_rect()
bullets = []
enemies = []
deaths = []
#pygame.mixer.music.play()
js = Joystick.Joystick()
js.main()
while (gameOver):
joyButton = js.readadc(2, js.SPICLK, js.SPIMOSI, js.SPIMISO,
js.SPICS)
if (joyButton > 1000):
gameOver = 0
for event in pygame.event.get():
if event.type == pygame.QUIT:
js.deactivateLED()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE:
gameOver = 0
elif event.key == pygame.K_ESCAPE or event.key == pygame.K_q:
js.deactivateLED()
sys.exit()
screen.blit(bg1, (0, 0))
screen.blit(title, (25, 20))
screen.blit(tx1, (40, 120))
pygame.display.flip()
clock.tick(60)
#activate the game
player.rect.y = 250
js.deactivateLED()
while (deathCounter < 50):
joyX = js.readadc(1, js.SPICLK, js.SPIMOSI, js.SPIMISO, js.SPICS)
joyY = js.readadc(0, js.SPICLK, js.SPIMOSI, js.SPIMISO, js.SPICS)
joyButton = js.readadc(2, js.SPICLK, js.SPIMOSI, js.SPIMISO,
js.SPICS)
#print("X:{0} Y:{1} B:{2}".format(joyX, joyY, joyButton))
if (joyButton > 1000 and buttonPressed == 0):
bullet = Bullets.BasicBullet(player.rect.x + 13, player.rect.y)
bullets.append(bullet)
buttonPressed = 1
js.setLED(1, True)
else:
buttonPressed = 0
js.setLED(1, False)
for event in pygame.event.get():
if event.type == pygame.QUIT:
js.deactivateLED()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE:
bullet = Bullets.BasicBullet(player.rect.x + 13,
player.rect.y)
#soundObj = pygame.mixer.Sound('sfx/blast.wav')
#soundObj.play()
bullets.append(bullet)
elif event.key == pygame.K_ESCAPE:
js.deactivateLED()
sys.exit()
elif event.key == pygame.K_e:
debug ^= 1
player.update(pygame.key.get_pressed(), joyX, joyY)
copterBlades.x = player.rect.x
copterBlades.y = player.rect.y
lblPosition = myfont.render(
"X:{0} Y:{1}".format(player.rect.x, player.rect.y), 1,
(0, 0, 0))
lblPosition2 = myfont.render("Bullets:{0}".format(len(bullets)), 1,
(0, 0, 0))
lblPosition3 = myfont.render("Time:{0}".format(timer), 1,
(0, 0, 0))
lblScore = myfont.render("score:{0}".format(score), 1,
(255, 255, 255))
# Enemy AI Bounce behavior
#if rect.left < 0 or rect.right > width:
# player.setSpeed(-player.getSpeed()[0], player.getSpeed()[1])
#if rect.top < 0 or rect.bottom > height:
# player.setSpeed(player.getSpeed()[0], -player.getSpeed()[1])
# Start drawing
screen.blit(bg1, (0, 0))
for bullet in bullets:
if (bullet.alive):
bullet.update()
screen.blit(bullet.getSprite(), bullet.rect)
else:
bullets.remove(bullet)
for enemy in enemies:
if (enemy.alive):
if (player.collision(enemy.rect.x,
enemy.rect.x + enemy.rect.width,
enemy.rect.y,
enemy.rect.y + enemy.rect.height)):
if (enemy.id != 2):
enemy.alive = 0
screen.blit(enemy.getSprite(), enemy.rect)
enemy.update()
for bullet in bullets:
if (bullet.alive):
if (enemy.collision(bullet.rect.x, bullet.rect.y)):
bullets.remove(bullet)
enemy.hp -= 1
else:
deaths.append(
Explosion.Explosion(enemy.rect.x, enemy.rect.y,
enemy.id))
score += enemy.score
enemies.remove(enemy)
if (enemy.rect.y > 350):
enemies.remove(enemy)
for explosion in deaths:
if (explosion.alive):
explosion.update()
screen.blit(explosion.getSprite(), explosion.rect)
else:
deaths.remove(explosion)
if (debug):
screen.blit(lblPosition, (5, 0))
screen.blit(lblPosition2, (5, 12))
screen.blit(lblPosition3, (5, 24))
if (player.alive):
screen.blit(player.getSprite(), player.rect)
screen.blit(player.getSpriteBlade(), copterBlades)
else:
deaths.append(
Explosion.Explosion(player.rect.x, player.rect.y, 0))
player.rect.y = -100
gameOver = 1
#display score
screen.blit(lblScore, (5, 288))
if (gameOver):
deathCounter += 1
screen.blit(title, (25, 20))
screen.blit(tx1, (40, 120))
js.setLED(2, True)
pygame.display.flip()
clock.tick(60)
timer = (timer + 1) % 50000
if (timer % 1000 == 0):
enemies.append(Enemy.Enemy(50, -200, 2))
elif (timer % 200 == 0):
enemies.append(Enemy.Enemy(random.randint(20, 180), 0, 1))
elif (timer % 350 == 0):
enemies.append(Enemy.Enemy(random.randint(20, 180), 0, 1))
elif (timer % 67 == 0):
enemies.append(Enemy.Enemy(random.randint(20, 180), 0, 1))
#while done, gameOver
self.main()