def executor():
""" Executes commands on the queue with specified time delay.
"""
# Get time delay
timedelay = int(config['output']['timedelay'])
# Loop to get messages
message = ''
while not message.strip().lower() in ['x', 'exit', 'q', 'quit']:
# Get message from queue and split into time and message
msg = queue.get()
timesec, message = msg.split(' ', 1)
timesec = float(timesec)
commsocket.send_log('Running: %s' % msg, logport, 'output.execute',
'DEBUG')
# Wait until it's timesec+timedelay
while time.time() - timesec < timedelay:
time.sleep(0.5)
### EXECUTE MESSAGES HERE
if message[0:2] == 'mp':
# Execute motor power command
pass
if message[0:2] == 'mt':
# Execute motor time command
pass
if message[0] == 's':
# Execute servo command
pass
commsocket.send_log('Executed: %s' % message, logport,
'output.execute', 'INFO')
# Sleep a bit to make sure all messages are out and threads can close
time.sleep(0.2)
def enqueue_responder(message):
""" Attaches the timestamp to the message and puts it on the queue
"""
# Generate timestamp (epoch seconds)
timesec = time.time()
# Put stamp and message on the queue
msg = "%f %s" % (timesec, message)
queue.put(msg)
# Generate log message
commsocket.send_log(message, logport, 'output.enqueue', 'DEBUG')
def dataserver_responder(message):
""" Returns the requested data
"""
key = message.strip()
try:
value = datafmt[key] % datadict[key]
except:
commsocket.send_log("Invalid Value Name %s" % key, logport,
'insensor.dataserver', 'WARN')
value = ''
return value
def getlogdata():
""" GetLogData thread function: Runs an infinite loop
reading all data in specified intervals. Data
is logged and stored in the dictionary.
"""
# Get time
nextime = time.time() + int(config['insense']['datarate'])
# Infinite loop
while True:
# Wait for interval
count = 0
while time.time() < nextime:
time.sleep(0.2)
count += 1
nextime = time.time() + int(config['insense']['datarate'])
commsocket.send_log(
"Done Waiting, Time=%d, Count=%d" % (nextime, count), logport,
'insensor.getlogdata', 'DEBUG')
### Read all data
# Time data
datadict['time'] = time.strftime('%H:%M:%S')
# Analog data
if mpc_on:
for i in range(Nanalog):
aname = 'analog%d' % (i + 1)
try:
#mcp = Adafruit_MCP3008.MCP3008(clk=CLK, cs=CS, miso=MISO, mosi=MOSI)
datadict[aname] = mcp.read_adc(i)
except Exception as e:
commsocket.send_log("Error Reading %s" % aname, logport,
'insensor.getlogdata', 'WARN')
print(str(e))
time.sleep(0.05)
# Attitude control data (BNO055 sensor)
if bno_on:
heading, roll, pitch = bno.read_euler()
datadict['heading'] = heading
datadict['roll'] = roll
datadict['pitch'] = pitch
time.sleep(0.05)
xgyro, ygyro, zgyro = bno.read_gyroscope()
datadict['xgyro'] = xgyro
datadict['ygyro'] = ygyro
datadict['zgyro'] = zgyro
### Save data to file
# Make text line
s = ''
for key in datalist:
s += datafmt[key] % datadict[key] + '\t'
s.strip()
# Add to file
datfile = open(datafilename, 'at')
datfile.write(s + '\n')
datfile.close()
def executor():
""" Executes commands on the queue with specified time delay.
"""
# Get time delay
timedelay = int(config['output']['timedelay'])
# Loop to get messages
message = ''
while not message.strip().lower() in ['x', 'exit', 'q', 'quit']:
# Get message from queue and split into time and message
msg = queue.get()
timesec, message = msg.split(' ', 1)
timesec = float(timesec)
commsocket.send_log('Running: %s' % msg, logport, 'output.execute', 'DEBUG')
# Wait until it's timesec+timedelay
while time.time() - timesec < timedelay :
time.sleep(0.5)
### EXECUTE MESSAGES HERE
# Motor:
if message[0].lower() == 'm':
# Make a motor hat with correct number
mh =Adafruit_MotorHAT(addr=motorHatAddr)
motor=mh.getMotor(int(message[1]))
# Get speed, direction and duration
msplit = message.split()
speed = int(msplit[1])
if speed < 0:
direction = Adafruit_MotorHAT.BACKWARD
speed = -speed
elif speed > 0:
direction = Adafruit_MotorHAT.FORWARD
else:
direction = Adafruit_MotorHAT.RELEASE
duration = float(msplit[2])
# Set motor movement
motor.run(direction)
motor.setSpeed(speed)
# Wait for duration
if duration!=0:
time.sleep(duration)
motor.run(Adafruit_MotorHAT.RELEASE)
# Servo
if message[0] == 's':
pwm = Adafruit_PCA9685.PCA9685(address=servoHatAddr)
pwm.set_pwm_freq(60)
pwm.set_pwm(int(message[1]),0,int(message[3:]))
time.sleep(1)
if message[0:5]=='color':
print("Color Sent")
incamport=int(config['ports']['flask_incam'])
commsocket.send_msg(message,incamport)
commsocket.send_log('Executed: %s' % message, logport, 'output.execute', 'INFO')
# Sleep a bit to make sure all messages are out and threads can close
time.sleep(0.2)
def send_command(command):
spaceloc = command.find(' ')
if spaceloc < 0: # if no space is found -> print error
print(" Error: Can't find 'target message' in command (you need a space between target and message)")
target = command[:spaceloc]
message = command[spaceloc+1:]
if target.lower() == 'output':
commsocket.send_msg(message, output_port)
elif target.lower() == 'insense':
response = commsocket.send_recv(message, insense_port)
flash(' Response from InSense: "%s"' % response)
elif target.lower() == 'incam':
commsocket.send_msg(message, incam_port)
elif target.lower() == 'log':
commsocket.send_log(message, log_port, 'flask.client')
else:
flash(' Error: "%s" is invalid target' % target)
# DataServer responder function
def dataserver_responder(message):
""" Returns the requested data
"""
key = message.strip()
try:
value = datafmt[key] % datadict[key]
except:
commsocket.send_log("Invalid Value Name %s" % key, logport,
'insensor.dataserver', 'WARN')
value = ''
return value
### Initialize threads_key
# Define data filename (run through time and expandvars)
datafilename = config['insense']['datafile']
datafilename = time.strftime(datafilename)
datafilename = os.path.expandvars(datafilename)
commsocket.send_log('Filename set to %s' % datafilename, logport,
'insensor.setup', 'INFO')
# Set up threads
getthread = threading.Thread(target=getlogdata)
serverthread = threading.Thread(target=dataserver)
getthread.daemon = True
getthread.start()
serverthread.start()
# Run until servethread shuts down
serverthread.join()
