#!/usr/bin/python3 # coding: Latin-1 # Load library functions we want import time import os import sys import pygame import PicoBorgRev3 as PicoBorgRev import picamera # Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame sys.stdout = sys.stderr # Global values global autoMode global running global PBR global controllerLost autoMode = False running = True controllerLost = False picture_directory = 'media/pictures' video_directory = 'media/videos' picture_count = 0o01 video_count = 0o01 vid_count = str(video_count) save_video_file = os.path.join(video_directory, 'video' + vid_count + '.h264') is_recording = False print('Libraries loaded') camera = picamera.PiCamera() def take_picture(): global picture_count pic_count = str(picture_count) first_picture_file = os.path.join(picture_directory, 'image' + pic_count + '.jpg') if os.path.exists(picture_directory): if os.path.isfile(first_picture_file): while os.path.exists('media/pictures/image%s.jpg' % picture_count): picture_count += 1 count_str = str(picture_count) new_picture_file = os.path.join(picture_directory, 'image' + count_str + '.jpg') print("taking picture...") time.sleep(1) camera.capture(new_picture_file) print('saved as : {0}'.format(new_picture_file)) else: print('taking picture...') time.sleep(1) camera.capture(first_picture_file) print('saved as : {0}'.format(first_picture_file)) def start_recording(): global video_count vid_count = str(video_count) first_video_file = os.path.join(video_directory, 'video' + vid_count + '.h264') if os.path.exists(video_directory): if os.path.isfile(first_video_file): while os.path.exists('media/videos/video%s.h264' % video_count): video_count += 1 video_str = str(video_count) new_video_file = os.path.join(video_directory, 'video' + video_str + '.h264') print("starting video recording...") time.sleep(2) camera.start_recording(new_video_file) print('saved as : {0}'.format(new_video_file)) else: print("starting video recording...") camera.start_recording(first_video_file) print('saved as: {0}'.format(first_video_file)) def stop_recording(): print('stopping recording...') camera.stop_recording() print('...recording stopped') # Setup the PicoBorg Reverse PBR = PicoBorgRev.PicoBorgRev() #PBR.i2cAddress = 0x44 # Uncomment and change the value if you have changed the board address PBR.Init() if not PBR.foundChip: boards = PicoBorgRev.ScanForPicoBorgReverse() if len(boards) == 0: print('No PicoBorg Reverse found, check you are attached :)') else: print('No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR.i2cAddress)) for board in boards: print(' %02X (%d)' % (board, board)) print('If you need to change the I²C address change the setup line so it is correct, e.g.') print('PBR.i2cAddress = 0x%02X' % (boards[0])) sys.exit() #PBR.SetEpoIgnore(True) # Uncomment to disable EPO latch, needed if you do not have a switch / jumper # Ensure the communications failsafe has been enabled! failsafe = False for i in range(5): PBR.SetCommsFailsafe(True) failsafe = PBR.GetCommsFailsafe() if failsafe: break if not failsafe: print('Board %02X failed to report in failsafe mode!' % (PBR.i2cAddress)) sys.exit() PBR.ResetEpo() # Settings for the joystick axisUpDown = 1 # Joystick axis to read for up / down position axisUpDownInverted = False # Set this to True if up and down appear to be swapped axisLeftRight = 2 # Joystick axis to read for left / right position axisLeftRightInverted = False # Set this to True if left and right appear to be swapped buttonResetEpo = 3 # Joystick button number to perform an EPO reset (Start) buttonSlow = 8 # Joystick button number for driving slowly whilst held (L2) slowFactor = 0.5 # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed buttonFastTurn = 9 # Joystick button number for turning fast (R2) interval = 0.02 # Time between updates in seconds, smaller responds faster but uses more processor time controllerLostLoops = 20 # Number of loops without any joystick events before announcing the joystick as out of range buttonSetAutoMode = 4 # Joystick button number to enable automatic control (D-Pad UP) buttonSetManualMode = 6 # Joystick button number to enable manual control (D-Pad DOWN) button_take_picture = 14 # take picture (X button) button_start_record = 12 # Start recording (Triangle Button) button_stop_record = 15 # stop recording (Square button) button_shutdown = 0 # shutdown (Select) # Power settings voltageIn = 12.0 # Total battery voltage to the PicoBorg Reverse voltageOut = 12.0 * 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power # Setup the power limits if voltageOut > voltageIn: maxPower = 1.0 else: maxPower = voltageOut / float(voltageIn) # Setup pygame and wait for the joystick to become available PBR.MotorsOff() os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window pygame.init() # pygame.display.set_mode((1,1)) print('Waiting for joystick... (press CTRL+C to abort)') while True: try: try: pygame.joystick.init() # Attempt to setup the joystick if pygame.joystick.get_count() < 1: # No joystick attached, toggle the LED PBR.SetLed(not PBR.GetLed()) pygame.joystick.quit() time.sleep(0.5) else: # We have a joystick, attempt to initialise it! joystick = pygame.joystick.Joystick(0) break except pygame.error: # Failed to connect to the joystick, toggle the LED PBR.SetLed(not PBR.GetLed()) pygame.joystick.quit() time.sleep(0.5) except KeyboardInterrupt: # CTRL+C exit, give up print('\nUser aborted') PBR.SetLed(True) sys.exit() print('Joystick found') joystick.init() PBR.SetLed(False) try: print('Press CTRL+C to quit') driveLeft = 0.0 driveRight = 0.0 hadEvent = False upDown = 0.0 leftRight = 0.0 loopsWithoutEvent = 0 # Loop indefinitely while running: # Get the latest events from the system hadEvent = False events = pygame.event.get() # Handle each event individually for event in events: if event.type == pygame.QUIT: # User exit running = False elif event.type == pygame.JOYBUTTONDOWN: # A button on the joystick just got pushed down if joystick.get_button(buttonSetAutoMode): print('setting auto mode...') autoMode = True if joystick.get_button(buttonSetManualMode): print('...stopping auto mode.') autoMode = False if joystick.get_button(buttonResetEpo): print('resetting EPO latch') PBR.ResetEpo() if joystick.get_button(button_take_picture): take_picture() if joystick.get_button(button_start_record): if not is_recording: start_recording() is_recording = True else: print('recording already in progress!') if joystick.get_button(button_stop_record): if is_recording: stop_recording() is_recording = False else: print('start recording 1st!') if joystick.get_button(button_shutdown): # Start the Raspberry Pi shutdown sequence print('Shutting down...') PBR.MotorsOff() PBR.SetCommsFailsafe(False) PBR.SetLed(True) os.system('sudo halt') break hadEvent = True elif event.type == pygame.JOYAXISMOTION: # A joystick has been moved hadEvent = True if hadEvent: if not autoMode: # Read axis positions (-1 to +1) if axisUpDownInverted: upDown = -joystick.get_axis(axisUpDown) else: upDown = joystick.get_axis(axisUpDown) if axisLeftRightInverted: leftRight = -joystick.get_axis(axisLeftRight) else: leftRight = joystick.get_axis(axisLeftRight) # Apply steering speeds if not joystick.get_button(buttonFastTurn): leftRight *= 0.5 # Determine the drive power levels driveLeft = -upDown driveRight = -upDown if leftRight < -0.05: # Turning left driveLeft *= 1.0 + (2.0 * leftRight) elif leftRight > 0.05: # Turning right driveRight *= 1.0 - (2.0 * leftRight) # Check for button presses if joystick.get_button(buttonSlow): driveLeft *= slowFactor driveRight *= slowFactor # Set the motors to the new speeds PBR.SetMotor1(driveRight * maxPower) PBR.SetMotor2(-driveLeft * maxPower) if hadEvent: # Reset the controller lost counter loopsWithoutEvent = 0 if controllerLost: # We had lost the controller, we have now found it again if autoMode: print('Controller re-connected, auto-motion will resume') else: print('Controller re-connected, move joystick to resume operation') PBR.SetLed(False) controllerLost = False elif controllerLost: # Controller has been lost, pulse the LED at a regular loop count loopsWithoutEvent += 1 if (loopsWithoutEvent % (controllerLostLoops / 10)) == 0: PBR.SetLed(not PBR.GetLed()) # Attempt to reset the joystick module del joystick pygame.joystick.quit() pygame.joystick.init() if pygame.joystick.get_count() < 1: # Controller has been disconnected, poll for reconnection print('Controller disconnected!') while pygame.joystick.get_count() < 1: time.sleep(interval * (controllerLostLoops / 10)) pygame.joystick.quit() pygame.joystick.init() PBR.SetLed(not PBR.GetLed()) # Grab the joystick again joystick = pygame.joystick.Joystick(0) joystick.init() continue # Skip to the next loop after the interval time.sleep(interval) continue else: # No events this loop, check if it has been too long since we saw an event loopsWithoutEvent += 1 if loopsWithoutEvent > controllerLostLoops: # It has been too long, disable control! print('Controller lost!') PBR.MotorsOff() PBR.SetLed(True) controllerLost = True # Skip to the next loop after the interval time.sleep(interval) continue # Change the LED to reflect the status of the EPO latch PBR.SetLed(PBR.GetEpo()) # Wait for the interval period time.sleep(interval) # Disable all drives PBR.MotorsOff() except KeyboardInterrupt: # CTRL+C exit, disable all drives print('\nUser shutdown') PBR.MotorsOff() except: # Unexpected error, shut down! e = sys.exc_info()[0] print() print(e) print('\nUnexpected error, shutting down!') PBR.MotorsOff() # Tell each thread to stop, and wait for them to end running = False PBR.SetLed(True) print('Program terminated.')