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/servo_2.py','left']) return def drive_right(): if not right: call(['python', './lib/servo_2.py','right']) return def reset_turn(): if left or right: call(['python','./lib/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) 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 ()