#!/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