mirror of https://github.com/Trivernis/spydian.git
You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
365 lines
10 KiB
Python
365 lines
10 KiB
Python
import pygame, picamera, ultrasonic#,gyro
|
|
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', 'servo_2.py','left'])
|
|
return
|
|
|
|
def drive_right():
|
|
if not right:
|
|
call(['python', 'servo_2.py','right'])
|
|
return
|
|
|
|
def reset_turn():
|
|
if left or right:
|
|
call(['python','servo_2.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)
|
|
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 ()
|