codepad
[
create a new paste
]
login
|
about
Language:
C
C++
D
Haskell
Lua
OCaml
PHP
Perl
Plain Text
Python
Ruby
Scheme
Tcl
#!/usr/bin/python3 # -*- coding: utf-8 -*- # # Main - Heart of the python code running on a RaspberryPI and driving an Arduino controlled Robot # # Version: 0.1 # Creator: meigrafd # Copyright (C) 2016 by meiraspi@gmail.com published under the MIT License # # # DONT use CTRL+C to Terminate, else Port is blocked # import time import sys import logging import multiprocessing as mp import RPi.GPIO as GPIO from optparse import OptionParser from pystalkd import Beanstalkd #------------------------------------------------------------------- sys.path.append("Toolsbox") logFileName = '/dev/shm/log.RoPi' # ***** # Get command line options # ***** parser = OptionParser() parser.add_option("-c", "--disableStream", dest="disableStream", action="store_true", help="Disable the camera stream") parser.add_option("-i", "--timingDisplay", action="store_true", dest="timingDisplay", help="Display timings from each major component of the processing loop") parser.add_option("-l", "--logging", dest="loggingLevel", default="I", help="Logging level of (D)ebug, (I)nfo, (W)arning, (E)rror, (C)ritical") parser.add_option("-m", "--map", dest="map", default="", help="Map to reload (date/time stamp portion only)") parser.add_option("-p", "--port", dest="serialPort", default="/dev/ttyACM0", help="Name of the serial port to use") parser.add_option("-b", "--baud", dest="serialBaudrate", default="57600", help="Baudrate of the serial port to use") parser.add_option("-r", "--router", dest="routerIP", default="192.168.0.1", help="Set the IP of our Router for network alive checks") parser.add_option("-s", "--shutdown", dest="shutdown", action="store_true", help="Sends a shutdown message to all processes") parser.add_option("-v", "--voltageCheck", dest="voltageCheck", action="store_true", help="Run a voltages check only") (options, args) = parser.parse_args() #------------------------------------------------------------------- # ***** # Setup logging # ***** logger = logging.getLogger("Main") # Create formatter formatter = logging.Formatter("%(levelname)s - %(message)s") # Send output to a file as well loggingFormat = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" logging.basicConfig(filename=logFileName, level=logging.INFO, format=loggingFormat) # Create console handler and set level to debug ch = logging.StreamHandler() # Set appropriate logging level based on options loggingLevels = { "D" : "DEBUG", "I" : "INFO", "W" : "WARNING", "E" : "ERROR", "C" : "CRITICAL" } eval("ch.setLevel(logging." + loggingLevels[options.loggingLevel] + ")") eval("logger.setLevel(logging." + loggingLevels[options.loggingLevel] + ")") # Add formatter to ch ch.setFormatter(formatter) # Add ch to logger logger.addHandler(ch) logger.debug("Initiating startup with the following options:") for key, val in vars(options).items(): logger.debug(" " + key + " = '" + str(val) + "'") #------------------------------------------------------------------- # ***** # Radar - Sweep and increment settings # ***** mapping = { "startOrient":70, "endOrient":-70, "increment":1, } # ***** # Pin assignments of Arduino - will be communicated. Mapped Pin Name # # http://arduino.cc/en/Hacking/PinMapping2560 # Rover 5 Explorer with Arduino Mega or DAGU Spider. # This tells the controller which pin is connected to which sensor, switch, motor etc. # You can re-define the pins here to suit your robot. This is your wiring diagram! # Pins are usually chosen by their function but in some cases by physical location (shorter wires). # You may not require all functions listed here, ignore those you do not need. # ***** # # Der Aufgruck auf dem Arduino neben den Buchsen entspricht dem "Mapped Pin Name" # pins = { # Pin assignments for robot power test #"powerRPi": 8, #"powerMain": 9, #"power5vBus": 6, #"powerRPiMain": 7, "startRobot": 14, # On to start the Robot - digital input D14 #"reset": 33, # Reset/Reboot Arduino - digital input D33 "UltraSonic_Trig": 24, # Ultrasonic HC-SR04 TRIGGER - digital output D24 "UltraSonic_Echo": 25, # Ultrasonic HC-SR04 ECHO - digital input D25 "ServoPan": 26, # PAN left or right - digital output D26 "ServoTilt": 27, # TILT up or down - digital output D27 "LeftDir": 29, # Left Motor Direction - digital output D29 "LeftSpeed": 5, # Left Motor Speed PWM - digital output D4 "LeftCur": "A6", # Left Motor Current - analog input A6 "LeftEnc1": 2, # Left Motor Encoder A - external Interrupt0 D2 "LeftEnc2": 3, # Left Motor Encoder B - external Interrupt1 D3 "RightDir": 28, # Right Motor Direction - digital output D28 "RightSpeed": 4, # Right Motor Speed PWM - digital output D5 "RightCur": "A7", # Right Motor Current - analog input A7 "RightEnc1": 18, # Right Motor Encoder A - external interrupt5 D18 "RightEnc2": 19, # Right Motor Encoder B - external interrupt4 D19 "FrontRightIRS": "A12", # Front Right IR Sensor - analog input A12 "FrontLeftIRS": "A13", # Front Left IR Sensor - analog input A13 "RearLeftIRS": "A14", # Rear Left IR Sensor - analog input A14 "RearRightIRS": "A15", # Rear Right IR Sensor - analog input A15 "FrontRightIRL": 8, # Front Right IR LEDs trigger - digital output D8 "FrontLeftIRL": 9, # Front Left IR LEDs - digital output D9 "RearLeftIRL": 10, # Rear Left IR LEDs - digital output D10 "RearRightIRL": 11, # Rear Right IR LEDs - digital output D11 "ErrorLED": 22, # Error LED - digital output D22 "pingLED": 23, # Ultrasonic activity LED - digital output D23 #"motorLED": 49, # Motor activity LED - digital output D49 #"Speaker": 44, # Speaker PWM - digital output D44 } special_pins = { "I2C_SDA": 20, # CMPS10 Compass Module "I2C_SCL": 21, "SPI_MISO": 50, "SPI_MOSI": 51, "SPI_SCK": 52, "SPI_SS": 53, } ## I2C Addresses: # CMPS10: 0x60 # EEPROM (24LC256): 0x50 ## CMPS10 Compass # needed: รก 1k8 or 4k7 resistors between: # Arduino-UNO pin A4 and 5V -> CMPS10 SDA # Arduino-UNO pin A5 and 5V -> CMPS10 SCL # Only A4 and A5 can be used for I2C! http://playground.arduino.cc/Learning/Pins # Arduino-Mega pin 20 and 5V -> CMPS10 SDA # Arduino-Mega pin 21 and 5V -> CMPS10 SCL #------------------------------------------------------------------- # ***** # Static Settings. Cant be changed from multiprocessing Processes # ***** settings = { "options": options, # options from the command line "pins": pins, # pin assignments on the Arduino "mapping": mapping, # stuff for the mapping module "distance_Min": 10, # Min distance between robot and objects from when it Turns (in centimeters) "distance_Max": 500, # Maximum distance before an object is considered "out of range" "distance_Best": 50, # Best distance to be from object so tracking won't be lost if object moves suddenly "distance_Slowdown": 30, # Min distance of an object from when robot slows down motor speed "PAN_Center": 1400, # Center position of pan servo in uS "PAN_Min": 700, # Pan servo lower limit in uS "PAN_Max": 2100, # Pan servo upper limit in uS "PAN_Scalafact": 4, # Scale factor to prevent pan servo overcorrecting "TILT_Center": 1500, # Center position of tilt servo in uS "TILT_Min": 700, # Tilt servo lower limit in uS "TILT_Max": 1750, # Tilt servo upper limit in uS "TILT_Scalafact": 4, # Scale factor to prevent tilt servo overcorrecting "ServoMaxPan": 170, # Max Pan servo angle in degrees. Don't go to very end of servo travel which may not be all the way from 0 to 180. "ServoMinPan": 0, # Min Pan servo angle in degrees. "ServoMaxTiltUP": 110, # Max Tilt UP position "ServoMaxTiltDOWN": 180, # Max Tilt DOWN position "ServoPanSteps": 30, "ServoTiltSteps": 30, "ServoWait": 330, # Milliseconds between Step Changes "UP": 1, "DOWN": 2, "RIGHT": 3, "LEFT": 4, "dir_pan": "RIGHT", # pan default direction "dir_tilt": "UP", # tilt default direction "start_pos_tilt": 150, "start_pos_pan": 70, "disdeadband": 40, # Distance deadband allows the object to change distance a small amount without robot reacting "pandeadband": 20, # Pan deadband allows head to pan a small amount before body begins tracking object "exploreSpeed": 150, # travel speed when robot is exploring "patience": 4000, # milliseconds without stimulation in playmode required to become bored "reversetime": 2000, # time to reverse/backward in milliseconds "MotorSpeed": 127, # default Motor Speed on startup "minMotorSpeed": 30, # minimal Motor Speed to prevent stall "CMPS10_addr": "0x60", # CMPS10 Compass I2C address (default: 0x60) "pystalkPort": 11300, "pystalkHost": "127.0.0.1", "qPrioL": 10, # pystalk Queue Priority Level: Low "qPrioM": 5, # pystalk Queue Priority Level: Mid "qPrioH": 1, # pystalk Queue Priority Level: High "webPort": 80, "streamWS_PORT": 8084, "streamWIDTH": 640, "streamHEIGHT": 480, "streamFRAMERATE": 24, "streamBRIGHTNESS": 55, # default: 50 "streamCONTRAST": 0, # default: 0 "audioWS_PORT": 9084, "vectorMapsPath": "maps/", } Beanstalkd.DEFAULT_PRIORITY=settings["qPrioM"] #------------------------------------------------------------------- # ***** # Dynamic settings. Can be changed from multiprocessing Processes # ***** # create shared dictionary - to share it between multiprocessing-processes status = mp.Manager().dict() status["updateMap"] = False status["statusMessage"] = "" status["restart"] = False status["shutdown"] = False status["maneuvering"] = False # robot maneuvering or not status["streaming"] = True # camera streaming status["stop"] = False # stop command status["forwardReverse"] = "F" # robot currently moving forward or reverse status["blockedForward"] = False # blocked going forward status["blockedReverse"] = False # blocked going in reverse status["lastTiming"] = 0 # Last time in milliseconds status["commandSignal"] = False # Signal that we have received a command #------------------------------------------------------------------- # ***** # Dynamic Telemetry Values # ***** # create shared dictionary - to share it between multiprocessing-processes telemetry = mp.Manager().dict() telemetry["powerMain"] = 0 telemetry["powerRPi"] = 0 telemetry["powerRPiMain"] = 0 telemetry["power5vBus"] = 0 telemetry["powerAdjust"] = 0 telemetry["distance"] = 0 telemetry["range"] = 0 telemetry["rangeAdjust"] = 1 telemetry["bearing"] = 0 # current robot bearing (rounded) telemetry["bearingRaw"] = 0 # current robot bearing (raw) telemetry["steerTo"] = 0 # commanded steer to bearing telemetry["course"] = 0 # current stable course bearing telemetry["pitch"] = 0 # Compass Pitch telemetry["roll"] = 0 # Compass Roll telemetry["IRfrontright"] = 0 telemetry["IRfrontleft"] = 0 telemetry["IRrearleft"] = 0 telemetry["IRrearright"] = 0 telemetry["LeftEncoder"] = 0 telemetry["RightEncoder"] = 0 telemetry["direction"] = 0 # direction command (1 = +5 degrees) telemetry["pos_pan"] = settings["start_pos_pan"] telemetry["pos_tilt"] = settings["start_pos_tilt"] telemetry["MotorSpeed"] = settings["MotorSpeed"] telemetry["leftspeed"] = settings["MotorSpeed"] telemetry["rightspeed"] = settings["MotorSpeed"] telemetry["lastTiming"] = 0 # Last time in milliseconds #------------------------------------------------------------------- # ***** # Import the classes # ***** from Toolsbox.USBinterface import USBinterfaceClass from Toolsbox.Utility import utilityClass from Toolsbox.RaspiCamDaemon import RaspiCamDaemonClass from Toolsbox.WebControlDaemon import WebControlDaemonClass from Toolsbox.WebControl import WebControlClass #from Toolsbox.Audio import AudioSpeakClass, AudioCommandClass #from Toolsbox.AudioDaemon import AudioDaemonClass from Toolsbox.Mapping import MappingClass from Toolsbox.Navigation import NavigationClass from Toolsbox.Radar import RadarClass from Toolsbox.Sensor import SensorClass from Toolsbox.Propulsion import PropulsionClass from Toolsbox.Proximity import ProximityClass #------------------------------------------------------------------- # ***** # Abort startup if the kill switch is pressed # ***** GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) # only one of following for GPIO.IN: PULL = GPIO.PUD_DOWN #GPIO -> GND #PULL = GPIO.PUD_UP #GPIO -> 3V3 GPIO.setup(settings["pins"]["startRobot"], GPIO.IN, pull_up_down=PULL) if GPIO.input(settings["pins"]["startRobot"]): logger.critical("Aborting startup - kill switch pressed.") GPIO.cleanup() sys.exit(1) # ***** # See if we are just here to kill ourselves # ***** if settings["options"].shutdown == True: logger.debug("Sending shutdown message.") queue = Beanstalkd.Connection(host=settings["pystalkHost"], port=settings["pystalkPort"]) queue.use("ControlCommand") queue.put("Terminate", priority=settings["qPrioH"]) sys.exit(0) #------------------------------------------------------------------- # Instantiate the interface logger.debug("Starting hardware interface.") arduino = USBinterfaceClass(port=settings["options"].serialPort, baudrate=settings["options"].serialBaudrate) arduino.LCD(0, "Robot starting") #speak = AudioSpeakClass(lang='en') #speak.tts("Starting.") utility = utilityClass(arduino, settings, status, telemetry) if arduino.initFailed: utility.logger("critical", "Failure initializing hardware interface.") utility.logger("critical", "Terminating startup.") sys.exit(1) utility.logger("debug", "Hardware interface has started.") if arduino.resyncHandshake(): utility.logger("critical", "Hardware interface is not responding to initial handshake.") utility.logger("critical", "Terminating startup.") sys.exit(1) # ***** # Get our reference voltage and display our various power levels # ***** referenceVoltage = utility.referenceVoltage() / 1000 utility.logger("debug", "Reference voltage at {0:.1f} volts".format(referenceVoltage)) settings["powerAdjust"] = 1 + ((referenceVoltage - 5) / 5) utility.logger("debug", "Reference adjustment {0:.3f}".format(settings["powerAdjust"])) utility.voltageCheck() if settings["options"].voltageCheck: sys.exit(1) arduino.LCD(0, "Interface up") arduino.LCD(1, " ") #------------------------------------------------------------------- # ***** # Open the queue manager and purge anything in the queues left from last execution # ***** utility.logger("debug", "Starting daemon communication queues.") # ControlTelemetry => Sensor Werte (auch fuers webif) # ControlCommand => zu verarbeitende Befehle fuer den Robot # WebCommand => zu verarbeitende Befehle vom WebIf # Utility => zur Steuerung diverser interner Tools queue = Beanstalkd.Connection(host=settings["pystalkHost"], port=settings["pystalkPort"]) try: if queue.stats_tube('ControlTelemetry')['current-jobs-ready'] > 0: utility.logger("debug", "Queue: Control Telemetry purged of " + str(utility.queuePurge("ControlTelemetry")) + " entries.") except: pass try: if queue.stats_tube('ControlCommand')['current-jobs-ready'] > 0: utility.logger("debug", "Queue: Control Command purged of " + str(utility.queuePurge("ControlCommand")) + " entries.") except: pass try: if queue.stats_tube('WebCommand')['current-jobs-ready'] > 0: utility.logger("debug", "Queue: Web Command purged of " + str(utility.queuePurge("WebCommand")) + " entries.") except: pass try: if queue.stats_tube('Utility')['current-jobs-ready']: utility.logger("debug", "Queue: Utility purged of " + str(utility.queuePurge("Utility")) + " entries.") except: pass logger.debug("Queues up.") arduino.LCD(0, "Queues up") # ***** # Stop streaming if so requested # ***** if settings["options"].disableStream: utility.disableStream() # ***** # Fire up the rest of the robot classes # ***** logger.debug("Starting Robot Classes ...") webcontrol = WebControlClass(arduino, utility, settings, status, telemetry) sensor = SensorClass(arduino, utility, settings, status, telemetry) radar = RadarClass(arduino, sensor, utility, settings, status, telemetry) navigation = NavigationClass(arduino, utility, settings, status, telemetry) mapping = MappingClass(arduino, sensor, radar, navigation, utility, settings, status, telemetry) propulsion = PropulsionClass(arduino, radar, navigation, utility, settings, status, telemetry) proximity = ProximityClass(arduino, propulsion, utility, settings, status, telemetry) # ***** # Start the background daemons and processes # ***** logger.debug("Starting Daemons ...") # Start the background camera process (if stream on) if not settings["options"].disableStream: cam = RaspiCamDaemonClass(utility, settings, status) camProcess = mp.Process(target=cam.run) camProcess.daemon = True camProcess.start() #audio = AudioDaemonClass(utility, settings, status) #audioRecord = mp.Process(target=audio.run) #audioRecord.daemon = True #audioRecord.start() web = WebControlDaemonClass(arduino, utility, settings, status, telemetry) webProcess = mp.Process(target=web.run) webProcess.daemon = True webProcess.start() logger.debug("Daemons up.") arduino.LCD(0, "Daemons up") # Pause following all of the activity above time.sleep(1) # ***** # Heartbeat loop and timer for telemetry updates # ***** iteration = 0 iterationMinute = -1 logger.info("Robot is waiting for commands.") arduino.LCD(0, "Robot started") speak.tts("Ready.") try: shutdown = False while not shutdown: time.sleep(0.001) # Check for commands from webif webcontrol.communicate() # Execute mapping orders if we are not maneuvering if status["maneuvering"] == False: mapping.execute() # Execute a navigation check navigation.execute() # Execute sensor orders - if we are moving the sensor will be pointed forward sensor.execute() # Send a telemetry broadcast every minute unless we are maneuvering if time.gmtime().tm_min > iterationMinute and status["maneuvering"] == False: iterationMinute = time.gmtime().tm_min sensor.detach() telemetry["range"] = radar.range() utility.statusUpdate() if utility.pingHome(): # Network down utility.signalStatus(1) arduino.LCD(0, "Network lost at") arduino.LCD(1, time.strftime("%H:%M:%S", time.gmtime())) utility.logger("critical", "Lost network at " + time.strftime("%H:%M:%S", time.gmtime())) else: arduino.LCD(0, "Robot running") # Are we done? if (status["shutdown"]): utility.logger("info", "Shutting down.") shutdown = True if (status["restart"]): utility.logger("info", "Shutting down for a restart.") shutdown = True # Update the iteration counter iteration += 1 if GPIO.input(settings["pins"]["startRobot"]): shutdown = True status["shutdown"] = True logger.critical("Stopping robot - kill switch pressed.") except (KeyboardInterrupt, SystemExit): pass finally: if camProcess: camProcess.terminate() if audioRecord: audioRecord.terminate() if webProcess: webProcess.terminate() # Do some cleanup utility.cleanup() if (status["shutdown"]): arduino.LCD(0, "Robot shutdown") sys.exit(1) else: arduino.LCD(0, "Robot restarting") #EOF
Private
[
?
]
Run code
Submit