mirror of https://github.com/Trivernis/spydian.git
- renamed main.py to main_old.py
- renamed main_new.py to main.py - added the camera functionalities - added the joystick to controllibpull/2/head
Trivernis
7 years ago
parent
2d11ea18ca
commit
5e4974327b
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,99 @@
|
|||||||
|
import pygame, os
|
||||||
|
|
||||||
|
class Joystick():
|
||||||
|
|
||||||
|
def __init__(self, navigator, light, configuration, camera):
|
||||||
|
#joystick
|
||||||
|
pygame.joystick.init()
|
||||||
|
joystick = pygame.joystick.Joystick(0)
|
||||||
|
joystick.init()
|
||||||
|
#sound
|
||||||
|
pygame.mixer.init()
|
||||||
|
sound = pygame.mixer.Sound('./sounds/gasgasgas.wav')
|
||||||
|
sound.set_volume(0.3)
|
||||||
|
self.camera = camera
|
||||||
|
self.sound = sound
|
||||||
|
self.joystick = joystick
|
||||||
|
self.navigator = navigator
|
||||||
|
self.light = light
|
||||||
|
self.configuration = configuration
|
||||||
|
self.pressed = []
|
||||||
|
self.splaying = False
|
||||||
|
|
||||||
|
def handle(self):
|
||||||
|
c = self.configuration
|
||||||
|
nav = self.navigator
|
||||||
|
dict = {}
|
||||||
|
#axes
|
||||||
|
for i in range(self.joystick.get_numaxes()):
|
||||||
|
axis = self.joystick.get_axis(i)
|
||||||
|
dict['Axis {}'.format(i)]=axis
|
||||||
|
|
||||||
|
if i==c['GAS'] and axis>0.1:
|
||||||
|
nav.forward()
|
||||||
|
elif i==c['GAS'] and axis<0.1:
|
||||||
|
nav.stop()
|
||||||
|
#buttons
|
||||||
|
for i in range(self.joystick.get_numbuttons()):
|
||||||
|
button = self.joystick.get_button(i)
|
||||||
|
dict['Button {}'.format(i)] = button
|
||||||
|
|
||||||
|
if i==c['LIGHT'] and button==1 and button not in self.pressed:
|
||||||
|
self.light.switch()
|
||||||
|
self.pressed.append(i)
|
||||||
|
elif i==c['LIGHT'] and button==0 and button in self.pressed:
|
||||||
|
self.pressed.remove(i)
|
||||||
|
|
||||||
|
elif i==c['MUSIC'] and button==1 and button not in self.pressed:
|
||||||
|
if self.splaying:
|
||||||
|
self.sound.stop()
|
||||||
|
self.splaying = False
|
||||||
|
else:
|
||||||
|
self.sound.play()
|
||||||
|
self.splaying = True
|
||||||
|
self.pressed.append(i)
|
||||||
|
elif i==c['MUSIC'] and button==0 and button in self.pressed:
|
||||||
|
self.pressed.remove(i)
|
||||||
|
|
||||||
|
elif i==c['VOLIN'] and button==1 and button not in self.pressed:
|
||||||
|
self.sound.set_volume(self.sound.get_volume()+0.1)
|
||||||
|
self.pressed.append(i)
|
||||||
|
elif i==c['VOLIN'] and button==0 and button in self.pressed:
|
||||||
|
self.sound.set_volume(self.sound.get_volume() - 0.1)
|
||||||
|
self.pressed.remove(i)
|
||||||
|
|
||||||
|
elif i==c['VOLDE'] and button==1 and button not in self.pressed:
|
||||||
|
self.pressed.append(i)
|
||||||
|
elif i==c['VOLDE'] and button==0 and button in self.pressed:
|
||||||
|
self.pressed.remove(i)
|
||||||
|
|
||||||
|
elif i==c['REC'] and button==1 and button not in self.pressed:
|
||||||
|
self._save_camimg()
|
||||||
|
self.pressed.append(i)
|
||||||
|
elif i==c['REC'] and button==0 and button in self.pressed:
|
||||||
|
self.pressed.remove(i)
|
||||||
|
|
||||||
|
#hats
|
||||||
|
for i in range(self.joystick.get_numhats()):
|
||||||
|
hat = self.joystick.get_hat(i)
|
||||||
|
dict['Hat {}'.format(i)] = hat
|
||||||
|
|
||||||
|
if hat==(-1,0):
|
||||||
|
nav.left()
|
||||||
|
elif hat==(1,0):
|
||||||
|
nav.right()
|
||||||
|
else:
|
||||||
|
nav.straight()
|
||||||
|
|
||||||
|
dict['Volume'] = self.sound.get_volume()
|
||||||
|
return dict
|
||||||
|
|
||||||
|
|
||||||
|
def _save_camimg(self):
|
||||||
|
img = self.camera.image
|
||||||
|
if os.path.isfile('image.jpg'):
|
||||||
|
count=0
|
||||||
|
while os.path.isfile('./images/img{}.jpg'.format(count)):
|
||||||
|
count+=1
|
||||||
|
os.rename('image.jpg', 'images/img{}.jpg'.format(count))
|
||||||
|
pygame.image.save(img, 'image.jpg')
|
||||||
@ -0,0 +1,81 @@
|
|||||||
|
import pygame, pygame.camera
|
||||||
|
from pygame import *
|
||||||
|
|
||||||
|
def render_text_line(image, color, font, text, pos = (0,0)):
|
||||||
|
render_text = font.render(text, 1, color)
|
||||||
|
image.blit(render_text, pos)
|
||||||
|
|
||||||
|
class Screen:
|
||||||
|
"""The Screen for the Terminal"""
|
||||||
|
|
||||||
|
def __init__(self, size = (100,100), title = "Screen"):
|
||||||
|
pygame.display.init()
|
||||||
|
self.size = size
|
||||||
|
self.screen = pygame.display.set_mode(self.size)
|
||||||
|
pygame.display.set_caption(title)
|
||||||
|
|
||||||
|
|
||||||
|
def refresh(self, rectangles=None):
|
||||||
|
pygame.display.update(rectangles)
|
||||||
|
|
||||||
|
|
||||||
|
def toggle_fullscreen(self):
|
||||||
|
if self.fullscreen:
|
||||||
|
pygame.display.set_mode(self.size)
|
||||||
|
self.fullscreen = False
|
||||||
|
else:
|
||||||
|
displayinfo = pygame.display.Info()
|
||||||
|
fullsize = (displayinfo.current_w, displayinfo.current_h)
|
||||||
|
pygame.display.set_mode(fullsize, FULLSCREEN | DOUBLEBUF)
|
||||||
|
self.fullscreen = True
|
||||||
|
|
||||||
|
|
||||||
|
class List(pygame.sprite.Sprite):
|
||||||
|
"""A List that shows the values of the terminal"""
|
||||||
|
|
||||||
|
def __init__(self, position, size):
|
||||||
|
pygame.sprite.Sprite.__init__(self)
|
||||||
|
pygame.font.init()
|
||||||
|
self.size = size
|
||||||
|
self.image = pygame.Surface(self.size)
|
||||||
|
self.image.fill((0,0,0))
|
||||||
|
self.rect = self.image.get_rect()
|
||||||
|
self.rect.topleft = position
|
||||||
|
self.font = pygame.font.SysFont('Arial',25)
|
||||||
|
self.dict = {}
|
||||||
|
self.updated = True
|
||||||
|
self.txtsize = self.font.size('__||__')
|
||||||
|
|
||||||
|
|
||||||
|
def set_dict(self, dict):
|
||||||
|
self.dict = dict
|
||||||
|
self.updated = True
|
||||||
|
|
||||||
|
def update(self, *args):
|
||||||
|
if self.updated:
|
||||||
|
height = 0
|
||||||
|
|
||||||
|
for key in self.dict.keys():
|
||||||
|
line = '{}: {}'.format(key, self.dict[key])
|
||||||
|
render_text_line(self.image, (255, 255, 255), self.font, line, (0,height))
|
||||||
|
height+=self.txtsize[1]
|
||||||
|
|
||||||
|
self.updated = False
|
||||||
|
|
||||||
|
class PiCamera(pygame.sprite.Sprite):
|
||||||
|
"""The Picamera as pygame cam"""
|
||||||
|
|
||||||
|
def __init__(self, position, size):
|
||||||
|
pygame.sprite.Sprite.__init__(self)
|
||||||
|
pygame.camera.init()
|
||||||
|
cam_list = pygame.camera.list_cameras()
|
||||||
|
camera = pygame.camera.Camera(cam_list[0], size)
|
||||||
|
self.camera = camera
|
||||||
|
self.size = size
|
||||||
|
self.image = pygame.Surface(self.size)
|
||||||
|
self.image.fill((0, 0, 0))
|
||||||
|
self.rect = self.image.get_rect()
|
||||||
|
self.rect.topleft = position
|
||||||
|
|
||||||
|
def update(self, *args):
|
||||||
|
self.camera.get_image(self.image)
|
||||||
@ -1,366 +1,46 @@
|
|||||||
import pygame, picamera #,gyro
|
import pygame, time
|
||||||
from lib import ultrasonic
|
from lib import graphiclib, hardwarelib, controllib
|
||||||
from datetime import datetime
|
|
||||||
from subprocess import call
|
configuration = {
|
||||||
import RPi.GPIO as GPIO
|
'GAS':5,
|
||||||
|
'STOP':2,
|
||||||
|
'MUSIC':1,
|
||||||
|
'LIGHT':0,
|
||||||
# Define some colors
|
'VOLIN':5,
|
||||||
BLACK = ( 0, 0, 0)
|
'VOLDE':4,
|
||||||
WHITE = ( 255, 255, 255)
|
'REC':3
|
||||||
# --ultrasonic--
|
}
|
||||||
sensor= ultrasonic.Sensor()
|
|
||||||
sensor.init(11,7)
|
def main():
|
||||||
# -- Nightmode
|
navigator = hardwarelib.Navigator(16)
|
||||||
# WHITE, BLACK=BLACK,WHITE
|
light = hardwarelib.Light(15)
|
||||||
# Pins BCM
|
ultrasonic = hardwarelib.Ultrasonic(11,7)
|
||||||
motor = 16
|
temperature = hardwarelib.Temperature()
|
||||||
lightpin=15
|
camera = graphiclib.PiCamera((500, 0), (500, 1000))
|
||||||
#
|
jstick = controllib.Joystick(navigator, light, configuration, camera)
|
||||||
# --config--
|
|
||||||
use_camera=False
|
#pygame stuff
|
||||||
use_gyro_preview=False
|
screen = graphiclib.Screen(size=(1000,1000))
|
||||||
#--
|
all_sprites = pygame.sprite.RenderUpdates()
|
||||||
forpress = False
|
list = graphiclib.List((0,0),(500,1000))
|
||||||
light = False
|
all_sprites.add(list)
|
||||||
lpress = False
|
all_sprites.add(camera)
|
||||||
play = False
|
clock = pygame.time.Clock()
|
||||||
ppress = False
|
running = True
|
||||||
voluppress = False
|
|
||||||
voldopress = False
|
while running:
|
||||||
recpress=False
|
clock.tick(25)
|
||||||
recording=False
|
for event in pygame.event.get():
|
||||||
left=False
|
if event.type == pygame.QUIT:
|
||||||
right=False
|
running = False
|
||||||
lastContact=datetime.now()
|
|
||||||
if use_camera:
|
dict = jstick.handle()
|
||||||
cam = picamera.PiCamera()
|
list.set_dict(dict)
|
||||||
|
all_sprites.update()
|
||||||
top_view = pygame.image.load('top_view.png')
|
update_rects = all_sprites.draw(screen.screen)
|
||||||
top_view= pygame.transform.scale(top_view, (192, 108))
|
screen.refresh(rectangles= update_rects)
|
||||||
|
pygame.quit()
|
||||||
# Axis
|
|
||||||
GAS = 5
|
|
||||||
STOP = 2
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
# Buttons
|
|
||||||
MUSIC = 1
|
|
||||||
LIGHT = 0
|
|
||||||
VOLUP = 5
|
|
||||||
VOLDO = 4
|
|
||||||
RECSTART = 3
|
|
||||||
|
|
||||||
# setup the relais
|
|
||||||
GPIO.setup(motor,GPIO.OUT)
|
|
||||||
GPIO.setup(lightpin,GPIO.OUT)
|
|
||||||
GPIO.output(motor,True)
|
|
||||||
GPIO.output(lightpin, True)
|
|
||||||
# ____________________________
|
|
||||||
# -- driving functions
|
|
||||||
def drive_forw():
|
|
||||||
GPIO.output(motor,False)
|
|
||||||
return
|
|
||||||
|
|
||||||
def drive_backw():
|
|
||||||
print(drive_backw)
|
|
||||||
return
|
|
||||||
|
|
||||||
def reset_dire():
|
|
||||||
GPIO.output(motor,True)
|
|
||||||
return
|
|
||||||
|
|
||||||
def drive_left():
|
|
||||||
if not left:
|
|
||||||
call(['python', './lib/servolib.py','left'])
|
|
||||||
return
|
|
||||||
|
|
||||||
def drive_right():
|
|
||||||
if not right:
|
|
||||||
call(['python', './lib/servolib.py','right'])
|
|
||||||
return
|
|
||||||
|
|
||||||
def reset_turn():
|
|
||||||
if left or right:
|
|
||||||
call(['python','./lib/servolib.py'])
|
|
||||||
return
|
|
||||||
# ____________________________
|
|
||||||
# -- special functions
|
|
||||||
def turn_light(shine):
|
|
||||||
if shine:
|
|
||||||
GPIO.output(lightpin, False)
|
|
||||||
else:
|
|
||||||
GPIO.output(lightpin, True)
|
|
||||||
return
|
|
||||||
|
|
||||||
def turn_rec(*record):
|
|
||||||
#if record:
|
|
||||||
#cam.start_recording('record.h264')
|
|
||||||
#return
|
|
||||||
#else:
|
|
||||||
#cam.stop_recording()
|
|
||||||
#return
|
|
||||||
if use_camera:
|
|
||||||
try:
|
|
||||||
cam.capture('image.jpg', resize=(192 * 2, 108 * 2))
|
|
||||||
cam.capture("./Images/image{}.jpg".format(str(datetime.now()).replace(':','_')))
|
|
||||||
except Exception as error:
|
|
||||||
print(error)
|
|
||||||
|
|
||||||
def imgRefresh():
|
|
||||||
#turn_rec(True)
|
|
||||||
image = None
|
|
||||||
try:
|
|
||||||
image = pygame.image.load('image.jpg')
|
|
||||||
image = pygame.transform.scale(image, (192*2, 108*2))
|
|
||||||
except:
|
|
||||||
pass
|
|
||||||
screen.blit(image, (500, 10))
|
|
||||||
#gyro_output = gyro.getAllOut()
|
|
||||||
#rotation = gyro_output['rot']
|
|
||||||
if use_gyro_preview:
|
|
||||||
top_view = pygame.image.load('top_view.png')
|
|
||||||
# The scaling of the top_view Spider
|
|
||||||
scal_x = abs(int(192 * ((abs(rotation[0]) / 90) - 1)))
|
|
||||||
scal_y = abs(int(108 * ((abs(rotation[1]) / 90) - 1)) - 1)
|
|
||||||
top_view = pygame.transform.scale(top_view, (scal_x, scal_y))
|
|
||||||
screen.blit(top_view, (550 - (scal_x / 2), 300 - (scal_y / 2)))
|
|
||||||
|
|
||||||
def printDriveData():
|
|
||||||
textPrint.print(screen,"")
|
|
||||||
textPrint.print(screen,"Sound Information")
|
|
||||||
textPrint.print(screen, " Sound: {}".format(play))
|
|
||||||
textPrint.print(screen, " Volume: {}".format(sound.get_volume()))
|
|
||||||
textPrint.print(screen, "Light: {}".format(light))
|
|
||||||
textPrint.print(screen, "Last Controller Input: {}".format((datetime.now()-lastContact).seconds))
|
|
||||||
#gyro_output=gyro.getAllOut()
|
|
||||||
#rotation=gyro_output['rot']
|
|
||||||
#textPrint.print(screen, "Rotation: X:{}; Y:{}".format(rotation[0],rotation[1]))
|
|
||||||
#acceleration = gyro_output['acc_sca']
|
|
||||||
#textPrint.print(screen, "Acceleration: X:{}; Y:{}; Z:{}".format(acceleration[0], acceleration[1],acceleration[2]))
|
|
||||||
distance=sensor.echo()
|
|
||||||
textPrint.print(screen, "Distance: {} cm".format(distance))
|
|
||||||
#if distance<40:
|
|
||||||
# turn_rec()
|
|
||||||
|
|
||||||
def doSubroutine():
|
|
||||||
if abs((datetime.now()-lastContact).seconds)==30:
|
|
||||||
print('Contact Lost')
|
|
||||||
imgRefresh()
|
|
||||||
|
|
||||||
# ____________________________
|
|
||||||
class TextPrint:
|
|
||||||
def __init__(self):
|
|
||||||
self.reset()
|
|
||||||
self.font = pygame.font.Font(None, 20)
|
|
||||||
|
|
||||||
def print(self, screen, textString):
|
|
||||||
textBitmap = self.font.render(textString, True, BLACK)
|
|
||||||
screen.blit(textBitmap, [self.x, self.y])
|
|
||||||
self.y += self.line_height
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self.x = 10
|
|
||||||
self.y = 10
|
|
||||||
self.line_height = 15
|
|
||||||
|
|
||||||
def indent(self):
|
|
||||||
self.x += 10
|
|
||||||
|
|
||||||
def unindent(self):
|
|
||||||
self.x -= 10
|
|
||||||
|
|
||||||
pygame.init()
|
|
||||||
|
|
||||||
# ________________________
|
|
||||||
# -- music playback
|
|
||||||
pygame.mixer.init()
|
|
||||||
sound=pygame.mixer.Sound('./gasgasgas.wav')
|
|
||||||
sound.set_volume(0.3)
|
|
||||||
# ________________________
|
|
||||||
|
|
||||||
# Set the width and height of the screen [width,height]
|
|
||||||
size = [1000, 700]
|
|
||||||
screen = pygame.display.set_mode(size)
|
|
||||||
|
|
||||||
pygame.display.set_caption("SPYDER STEUERZENTRALE")
|
|
||||||
|
|
||||||
# Loop until the user clicks the close button.
|
|
||||||
done = False
|
|
||||||
|
|
||||||
# Used to manage how fast the screen updates
|
|
||||||
clock = pygame.time.Clock()
|
|
||||||
|
|
||||||
# Initialize the joysticks
|
|
||||||
pygame.joystick.init()
|
|
||||||
|
|
||||||
# Get ready to print
|
|
||||||
textPrint = TextPrint()
|
|
||||||
turn_rec(True)
|
|
||||||
image=pygame.image.load('image.jpg')
|
|
||||||
image=pygame.transform.scale(image,(192*2,108*2))
|
|
||||||
count=0
|
|
||||||
|
|
||||||
# -------- Main Program Loop -----------
|
|
||||||
while done==False:
|
|
||||||
# EVENT PROCESSING STEP
|
|
||||||
for event in pygame.event.get(): # User did something
|
|
||||||
if event.type == pygame.QUIT: # If user clicked close
|
|
||||||
done=True # Flag that we are done so we exit this loop
|
|
||||||
|
|
||||||
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTIO
|
|
||||||
|
|
||||||
|
|
||||||
# DRAWING STEP
|
|
||||||
# First, clear the screen to white. Don't put other drawing commands
|
|
||||||
# above this, or they will be erased with this command.
|
|
||||||
screen.fill(WHITE)
|
|
||||||
textPrint.reset()
|
|
||||||
|
|
||||||
# Get count of joysticks
|
|
||||||
joystick_count = pygame.joystick.get_count()
|
|
||||||
|
|
||||||
textPrint.print(screen, "Number of joysticks: {}".format(joystick_count) )
|
|
||||||
textPrint.indent()
|
|
||||||
|
|
||||||
# For each joystick:
|
|
||||||
for i in range(joystick_count):
|
|
||||||
joystick = pygame.joystick.Joystick(i)
|
|
||||||
joystick.init()
|
|
||||||
|
|
||||||
textPrint.print(screen, "Joystick {}".format(i) )
|
|
||||||
textPrint.indent()
|
|
||||||
|
|
||||||
# Get the name from the OS for the controller/joystick
|
|
||||||
#name = joystick.get_name()
|
|
||||||
#textPrint.print(screen, "Joystick name: {}".format(name) )
|
|
||||||
|
|
||||||
# Usually axis run in pairs, up/down for one, and left/right for
|
|
||||||
# the other.
|
|
||||||
axes = joystick.get_numaxes()
|
|
||||||
textPrint.print(screen, "Number of axes: {}".format(axes) )
|
|
||||||
textPrint.indent()
|
|
||||||
|
|
||||||
for i in range( axes ):
|
|
||||||
axis = joystick.get_axis( i )
|
|
||||||
textPrint.print(screen, "Axis {} value: {:>6.3f}".format(i, axis) )
|
|
||||||
if i==STOP and axis>0.1:
|
|
||||||
print('STOP')
|
|
||||||
drive_backw()
|
|
||||||
lastContact=datetime.now()
|
|
||||||
elif i==GAS and axis>0.1:
|
|
||||||
print('GAS')
|
|
||||||
if not forpress:
|
|
||||||
drive_forw()
|
|
||||||
forpress=True
|
|
||||||
lastContact=datetime.now()
|
|
||||||
elif i==GAS and axis<0.1:
|
|
||||||
if forpress:
|
|
||||||
reset_dire()
|
|
||||||
print('RESETDRIVE')
|
|
||||||
forpress=False
|
|
||||||
|
|
||||||
textPrint.unindent()
|
|
||||||
|
|
||||||
buttons = joystick.get_numbuttons()
|
|
||||||
textPrint.print(screen, "Number of buttons: {}".format(buttons) )
|
|
||||||
textPrint.indent()
|
|
||||||
|
|
||||||
for i in range( buttons ):
|
|
||||||
button = joystick.get_button( i )
|
|
||||||
textPrint.print(screen, "Button {:>2} value: {}".format(i,button) )
|
|
||||||
if button==1:
|
|
||||||
lastContact=datetime.now()
|
|
||||||
#if i==7 and button==1:
|
|
||||||
# print('GAS')
|
|
||||||
# drive_forw()
|
|
||||||
#elif i==6 and button==1:
|
|
||||||
# print('STOP')
|
|
||||||
# drive_backw()
|
|
||||||
if i==LIGHT and button==1 and not lpress:
|
|
||||||
light=not light
|
|
||||||
lpress=True
|
|
||||||
turn_light(light)
|
|
||||||
print('Light: {}'.format(light))
|
|
||||||
elif i==LIGHT and button==0:
|
|
||||||
lpress=False
|
|
||||||
elif i==MUSIC and button==1 and not play and not ppress:
|
|
||||||
ppress=True
|
|
||||||
sound.play()
|
|
||||||
play=True
|
|
||||||
print('Sound: {}'.format(play))
|
|
||||||
elif i==MUSIC and button==1 and play and not ppress:
|
|
||||||
ppress=True
|
|
||||||
sound.stop()
|
|
||||||
play=False
|
|
||||||
print('Sound: {}'.format(play))
|
|
||||||
elif i==MUSIC and button==0 and ppress:
|
|
||||||
ppress=False
|
|
||||||
elif i==VOLUP and button==1 and not voluppress:
|
|
||||||
sound.set_volume(sound.get_volume()+0.1)
|
|
||||||
print('Volume: {}'.format(sound.get_volume()))
|
|
||||||
voluppress=True
|
|
||||||
elif i==VOLUP and button==0:
|
|
||||||
voluppress=False
|
|
||||||
elif i== VOLDO and button==0:
|
|
||||||
voldopress=False
|
|
||||||
elif i==VOLDO and button==1 and not voldopress:
|
|
||||||
sound.set_volume(sound.get_volume()-0.1)
|
|
||||||
print('Volume: {}'.format(sound.get_volume()))
|
|
||||||
voldopress=True
|
|
||||||
elif i==RECSTART and button==1 and not recpress:
|
|
||||||
recording=not recording
|
|
||||||
turn_rec(recording)
|
|
||||||
print('Recording: {}'.format(recording))
|
|
||||||
recpress=True
|
|
||||||
elif i==RECSTART and button==0:
|
|
||||||
recpress=False
|
|
||||||
|
|
||||||
textPrint.unindent()
|
|
||||||
|
|
||||||
# Hat switch. All or nothing for direction, not like joysticks.
|
|
||||||
# Value comes back in an array.
|
|
||||||
hats = joystick.get_numhats()
|
|
||||||
textPrint.print(screen, "Number of hats: {}".format(hats) )
|
|
||||||
textPrint.indent()
|
|
||||||
|
|
||||||
for i in range( hats ):
|
|
||||||
hat = joystick.get_hat( i )
|
|
||||||
textPrint.print(screen, "Hat {} value: {}".format(i, str(hat)) )
|
|
||||||
if hat==(-1,0):
|
|
||||||
print('LEFT')
|
|
||||||
drive_left()
|
|
||||||
left=True
|
|
||||||
right=False
|
|
||||||
lastContact=datetime.now()
|
|
||||||
elif hat==(1,0):
|
|
||||||
print('RIGHT')
|
|
||||||
drive_right()
|
|
||||||
right=True
|
|
||||||
left=False
|
|
||||||
lastContact=datetime.now()
|
|
||||||
else:
|
|
||||||
reset_turn()
|
|
||||||
left=False
|
|
||||||
right=False
|
|
||||||
textPrint.unindent()
|
|
||||||
|
|
||||||
textPrint.unindent()
|
|
||||||
|
|
||||||
printDriveData()
|
|
||||||
doSubroutine()
|
|
||||||
|
|
||||||
|
|
||||||
# ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT
|
|
||||||
|
|
||||||
# Go ahead and update the screen with what we've drawn.
|
|
||||||
pygame.display.flip()
|
|
||||||
|
|
||||||
# Limit to 20 frames per second
|
|
||||||
clock.tick(20)
|
|
||||||
|
|
||||||
# Close the window and quit.
|
|
||||||
# If you forget this line, the program will 'hang'
|
|
||||||
# on exit if running from IDLE.
|
|
||||||
sensor.clean()
|
|
||||||
pygame.quit ()
|
|
||||||
@ -1,9 +0,0 @@
|
|||||||
import pygame, time
|
|
||||||
from lib import ultrasonic
|
|
||||||
|
|
||||||
def main():
|
|
||||||
pygame.display.init()
|
|
||||||
pass
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@ -0,0 +1,366 @@
|
|||||||
|
import pygame, picamera #,gyro
|
||||||
|
from lib import ultrasonic
|
||||||
|
from datetime import datetime
|
||||||
|
from subprocess import call
|
||||||
|
import RPi.GPIO as GPIO
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Define some colors
|
||||||
|
BLACK = ( 0, 0, 0)
|
||||||
|
WHITE = ( 255, 255, 255)
|
||||||
|
# --ultrasonic--
|
||||||
|
sensor= ultrasonic.Sensor()
|
||||||
|
sensor.init(11,7)
|
||||||
|
# -- Nightmode
|
||||||
|
# WHITE, BLACK=BLACK,WHITE
|
||||||
|
# Pins BCM
|
||||||
|
motor = 16
|
||||||
|
lightpin=15
|
||||||
|
#
|
||||||
|
# --config--
|
||||||
|
use_camera=False
|
||||||
|
use_gyro_preview=False
|
||||||
|
#--
|
||||||
|
forpress = False
|
||||||
|
light = False
|
||||||
|
lpress = False
|
||||||
|
play = False
|
||||||
|
ppress = False
|
||||||
|
voluppress = False
|
||||||
|
voldopress = False
|
||||||
|
recpress=False
|
||||||
|
recording=False
|
||||||
|
left=False
|
||||||
|
right=False
|
||||||
|
lastContact=datetime.now()
|
||||||
|
if use_camera:
|
||||||
|
cam = picamera.PiCamera()
|
||||||
|
|
||||||
|
top_view = pygame.image.load('top_view.png')
|
||||||
|
top_view= pygame.transform.scale(top_view, (192, 108))
|
||||||
|
|
||||||
|
# Axis
|
||||||
|
GAS = 5
|
||||||
|
STOP = 2
|
||||||
|
|
||||||
|
# Buttons
|
||||||
|
MUSIC = 1
|
||||||
|
LIGHT = 0
|
||||||
|
VOLUP = 5
|
||||||
|
VOLDO = 4
|
||||||
|
RECSTART = 3
|
||||||
|
|
||||||
|
# setup the relais
|
||||||
|
GPIO.setup(motor,GPIO.OUT)
|
||||||
|
GPIO.setup(lightpin,GPIO.OUT)
|
||||||
|
GPIO.output(motor,True)
|
||||||
|
GPIO.output(lightpin, True)
|
||||||
|
# ____________________________
|
||||||
|
# -- driving functions
|
||||||
|
def drive_forw():
|
||||||
|
GPIO.output(motor,False)
|
||||||
|
return
|
||||||
|
|
||||||
|
def drive_backw():
|
||||||
|
print(drive_backw)
|
||||||
|
return
|
||||||
|
|
||||||
|
def reset_dire():
|
||||||
|
GPIO.output(motor,True)
|
||||||
|
return
|
||||||
|
|
||||||
|
def drive_left():
|
||||||
|
if not left:
|
||||||
|
call(['python', './lib/servolib.py','left'])
|
||||||
|
return
|
||||||
|
|
||||||
|
def drive_right():
|
||||||
|
if not right:
|
||||||
|
call(['python', './lib/servolib.py','right'])
|
||||||
|
return
|
||||||
|
|
||||||
|
def reset_turn():
|
||||||
|
if left or right:
|
||||||
|
call(['python','./lib/servolib.py'])
|
||||||
|
return
|
||||||
|
# ____________________________
|
||||||
|
# -- special functions
|
||||||
|
def turn_light(shine):
|
||||||
|
if shine:
|
||||||
|
GPIO.output(lightpin, False)
|
||||||
|
else:
|
||||||
|
GPIO.output(lightpin, True)
|
||||||
|
return
|
||||||
|
|
||||||
|
def turn_rec(*record):
|
||||||
|
#if record:
|
||||||
|
#cam.start_recording('record.h264')
|
||||||
|
#return
|
||||||
|
#else:
|
||||||
|
#cam.stop_recording()
|
||||||
|
#return
|
||||||
|
if use_camera:
|
||||||
|
try:
|
||||||
|
cam.capture('image.jpg', resize=(192 * 2, 108 * 2))
|
||||||
|
cam.capture("./Images/image{}.jpg".format(str(datetime.now()).replace(':','_')))
|
||||||
|
except Exception as error:
|
||||||
|
print(error)
|
||||||
|
|
||||||
|
def imgRefresh():
|
||||||
|
#turn_rec(True)
|
||||||
|
image = None
|
||||||
|
try:
|
||||||
|
image = pygame.image.load('image.jpg')
|
||||||
|
image = pygame.transform.scale(image, (192*2, 108*2))
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
screen.blit(image, (500, 10))
|
||||||
|
#gyro_output = gyro.getAllOut()
|
||||||
|
#rotation = gyro_output['rot']
|
||||||
|
if use_gyro_preview:
|
||||||
|
top_view = pygame.image.load('top_view.png')
|
||||||
|
# The scaling of the top_view Spider
|
||||||
|
scal_x = abs(int(192 * ((abs(rotation[0]) / 90) - 1)))
|
||||||
|
scal_y = abs(int(108 * ((abs(rotation[1]) / 90) - 1)) - 1)
|
||||||
|
top_view = pygame.transform.scale(top_view, (scal_x, scal_y))
|
||||||
|
screen.blit(top_view, (550 - (scal_x / 2), 300 - (scal_y / 2)))
|
||||||
|
|
||||||
|
def printDriveData():
|
||||||
|
textPrint.print(screen,"")
|
||||||
|
textPrint.print(screen,"Sound Information")
|
||||||
|
textPrint.print(screen, " Sound: {}".format(play))
|
||||||
|
textPrint.print(screen, " Volume: {}".format(sound.get_volume()))
|
||||||
|
textPrint.print(screen, "Light: {}".format(light))
|
||||||
|
textPrint.print(screen, "Last Controller Input: {}".format((datetime.now()-lastContact).seconds))
|
||||||
|
#gyro_output=gyro.getAllOut()
|
||||||
|
#rotation=gyro_output['rot']
|
||||||
|
#textPrint.print(screen, "Rotation: X:{}; Y:{}".format(rotation[0],rotation[1]))
|
||||||
|
#acceleration = gyro_output['acc_sca']
|
||||||
|
#textPrint.print(screen, "Acceleration: X:{}; Y:{}; Z:{}".format(acceleration[0], acceleration[1],acceleration[2]))
|
||||||
|
distance=sensor.echo()
|
||||||
|
textPrint.print(screen, "Distance: {} cm".format(distance))
|
||||||
|
#if distance<40:
|
||||||
|
# turn_rec()
|
||||||
|
|
||||||
|
def doSubroutine():
|
||||||
|
if abs((datetime.now()-lastContact).seconds)==30:
|
||||||
|
print('Contact Lost')
|
||||||
|
imgRefresh()
|
||||||
|
|
||||||
|
# ____________________________
|
||||||
|
class TextPrint:
|
||||||
|
def __init__(self):
|
||||||
|
self.reset()
|
||||||
|
self.font = pygame.font.Font(None, 20)
|
||||||
|
|
||||||
|
def print(self, screen, textString):
|
||||||
|
textBitmap = self.font.render(textString, True, BLACK)
|
||||||
|
screen.blit(textBitmap, [self.x, self.y])
|
||||||
|
self.y += self.line_height
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self.x = 10
|
||||||
|
self.y = 10
|
||||||
|
self.line_height = 15
|
||||||
|
|
||||||
|
def indent(self):
|
||||||
|
self.x += 10
|
||||||
|
|
||||||
|
def unindent(self):
|
||||||
|
self.x -= 10
|
||||||
|
|
||||||
|
pygame.init()
|
||||||
|
|
||||||
|
# ________________________
|
||||||
|
# -- music playback
|
||||||
|
pygame.mixer.init()
|
||||||
|
sound=pygame.mixer.Sound('./sounds/gasgasgas.wav')
|
||||||
|
sound.set_volume(0.3)
|
||||||
|
# ________________________
|
||||||
|
|
||||||
|
# Set the width and height of the screen [width,height]
|
||||||
|
size = [1000, 700]
|
||||||
|
screen = pygame.display.set_mode(size)
|
||||||
|
|
||||||
|
pygame.display.set_caption("SPYDER STEUERZENTRALE")
|
||||||
|
|
||||||
|
# Loop until the user clicks the close button.
|
||||||
|
done = False
|
||||||
|
|
||||||
|
# Used to manage how fast the screen updates
|
||||||
|
clock = pygame.time.Clock()
|
||||||
|
|
||||||
|
# Initialize the joysticks
|
||||||
|
pygame.joystick.init()
|
||||||
|
|
||||||
|
# Get ready to print
|
||||||
|
textPrint = TextPrint()
|
||||||
|
turn_rec(True)
|
||||||
|
image=pygame.image.load('image.jpg')
|
||||||
|
image=pygame.transform.scale(image,(192*2,108*2))
|
||||||
|
count=0
|
||||||
|
|
||||||
|
# -------- Main Program Loop -----------
|
||||||
|
while done==False:
|
||||||
|
# EVENT PROCESSING STEP
|
||||||
|
for event in pygame.event.get(): # User did something
|
||||||
|
if event.type == pygame.QUIT: # If user clicked close
|
||||||
|
done=True # Flag that we are done so we exit this loop
|
||||||
|
|
||||||
|
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTIO
|
||||||
|
|
||||||
|
|
||||||
|
# DRAWING STEP
|
||||||
|
# First, clear the screen to white. Don't put other drawing commands
|
||||||
|
# above this, or they will be erased with this command.
|
||||||
|
screen.fill(WHITE)
|
||||||
|
textPrint.reset()
|
||||||
|
|
||||||
|
# Get count of joysticks
|
||||||
|
joystick_count = pygame.joystick.get_count()
|
||||||
|
|
||||||
|
textPrint.print(screen, "Number of joysticks: {}".format(joystick_count) )
|
||||||
|
textPrint.indent()
|
||||||
|
|
||||||
|
# For each joystick:
|
||||||
|
for i in range(joystick_count):
|
||||||
|
joystick = pygame.joystick.Joystick(i)
|
||||||
|
joystick.init()
|
||||||
|
|
||||||
|
textPrint.print(screen, "Joystick {}".format(i) )
|
||||||
|
textPrint.indent()
|
||||||
|
|
||||||
|
# Get the name from the OS for the controller/joystick
|
||||||
|
#name = joystick.get_name()
|
||||||
|
#textPrint.print(screen, "Joystick name: {}".format(name) )
|
||||||
|
|
||||||
|
# Usually axis run in pairs, up/down for one, and left/right for
|
||||||
|
# the other.
|
||||||
|
axes = joystick.get_numaxes()
|
||||||
|
textPrint.print(screen, "Number of axes: {}".format(axes) )
|
||||||
|
textPrint.indent()
|
||||||
|
|
||||||
|
for i in range( axes ):
|
||||||
|
axis = joystick.get_axis( i )
|
||||||
|
textPrint.print(screen, "Axis {} value: {:>6.3f}".format(i, axis) )
|
||||||
|
if i==STOP and axis>0.1:
|
||||||
|
print('STOP')
|
||||||
|
drive_backw()
|
||||||
|
lastContact=datetime.now()
|
||||||
|
elif i==GAS and axis>0.1:
|
||||||
|
print('GAS')
|
||||||
|
if not forpress:
|
||||||
|
drive_forw()
|
||||||
|
forpress=True
|
||||||
|
lastContact=datetime.now()
|
||||||
|
elif i==GAS and axis<0.1:
|
||||||
|
if forpress:
|
||||||
|
reset_dire()
|
||||||
|
print('RESETDRIVE')
|
||||||
|
forpress=False
|
||||||
|
|
||||||
|
textPrint.unindent()
|
||||||
|
|
||||||
|
buttons = joystick.get_numbuttons()
|
||||||
|
textPrint.print(screen, "Number of buttons: {}".format(buttons) )
|
||||||
|
textPrint.indent()
|
||||||
|
|
||||||
|
for i in range( buttons ):
|
||||||
|
button = joystick.get_button( i )
|
||||||
|
textPrint.print(screen, "Button {:>2} value: {}".format(i,button) )
|
||||||
|
if button==1:
|
||||||
|
lastContact=datetime.now()
|
||||||
|
#if i==7 and button==1:
|
||||||
|
# print('GAS')
|
||||||
|
# drive_forw()
|
||||||
|
#elif i==6 and button==1:
|
||||||
|
# print('STOP')
|
||||||
|
# drive_backw()
|
||||||
|
if i==LIGHT and button==1 and not lpress:
|
||||||
|
light=not light
|
||||||
|
lpress=True
|
||||||
|
turn_light(light)
|
||||||
|
print('Light: {}'.format(light))
|
||||||
|
elif i==LIGHT and button==0:
|
||||||
|
lpress=False
|
||||||
|
elif i==MUSIC and button==1 and not play and not ppress:
|
||||||
|
ppress=True
|
||||||
|
sound.play()
|
||||||
|
play=True
|
||||||
|
print('Sound: {}'.format(play))
|
||||||
|
elif i==MUSIC and button==1 and play and not ppress:
|
||||||
|
ppress=True
|
||||||
|
sound.stop()
|
||||||
|
play=False
|
||||||
|
print('Sound: {}'.format(play))
|
||||||
|
elif i==MUSIC and button==0 and ppress:
|
||||||
|
ppress=False
|
||||||
|
elif i==VOLUP and button==1 and not voluppress:
|
||||||
|
sound.set_volume(sound.get_volume()+0.1)
|
||||||
|
print('Volume: {}'.format(sound.get_volume()))
|
||||||
|
voluppress=True
|
||||||
|
elif i==VOLUP and button==0:
|
||||||
|
voluppress=False
|
||||||
|
elif i== VOLDO and button==0:
|
||||||
|
voldopress=False
|
||||||
|
elif i==VOLDO and button==1 and not voldopress:
|
||||||
|
sound.set_volume(sound.get_volume()-0.1)
|
||||||
|
print('Volume: {}'.format(sound.get_volume()))
|
||||||
|
voldopress=True
|
||||||
|
elif i==RECSTART and button==1 and not recpress:
|
||||||
|
recording=not recording
|
||||||
|
turn_rec(recording)
|
||||||
|
print('Recording: {}'.format(recording))
|
||||||
|
recpress=True
|
||||||
|
elif i==RECSTART and button==0:
|
||||||
|
recpress=False
|
||||||
|
|
||||||
|
textPrint.unindent()
|
||||||
|
|
||||||
|
# Hat switch. All or nothing for direction, not like joysticks.
|
||||||
|
# Value comes back in an array.
|
||||||
|
hats = joystick.get_numhats()
|
||||||
|
textPrint.print(screen, "Number of hats: {}".format(hats) )
|
||||||
|
textPrint.indent()
|
||||||
|
|
||||||
|
for i in range( hats ):
|
||||||
|
hat = joystick.get_hat( i )
|
||||||
|
textPrint.print(screen, "Hat {} value: {}".format(i, str(hat)) )
|
||||||
|
if hat==(-1,0):
|
||||||
|
print('LEFT')
|
||||||
|
drive_left()
|
||||||
|
left=True
|
||||||
|
right=False
|
||||||
|
lastContact=datetime.now()
|
||||||
|
elif hat==(1,0):
|
||||||
|
print('RIGHT')
|
||||||
|
drive_right()
|
||||||
|
right=True
|
||||||
|
left=False
|
||||||
|
lastContact=datetime.now()
|
||||||
|
else:
|
||||||
|
reset_turn()
|
||||||
|
left=False
|
||||||
|
right=False
|
||||||
|
textPrint.unindent()
|
||||||
|
|
||||||
|
textPrint.unindent()
|
||||||
|
|
||||||
|
printDriveData()
|
||||||
|
doSubroutine()
|
||||||
|
|
||||||
|
|
||||||
|
# ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT
|
||||||
|
|
||||||
|
# Go ahead and update the screen with what we've drawn.
|
||||||
|
pygame.display.flip()
|
||||||
|
|
||||||
|
# Limit to 20 frames per second
|
||||||
|
clock.tick(20)
|
||||||
|
|
||||||
|
# Close the window and quit.
|
||||||
|
# If you forget this line, the program will 'hang'
|
||||||
|
# on exit if running from IDLE.
|
||||||
|
sensor.clean()
|
||||||
|
pygame.quit ()
|
||||||
Loading…
Reference in New Issue