#!/usr/bin/env python
"""position_regulation.py
Apply proportional control using sensor feedback and pneumatic valve activations
using a serial port connection to an Arduino running ValveControl.
Copyright (c) 2015-2017, Garth Zeglin. All rights reserved. Licensed under the
terms of the BSD 3-clause license.
"""
################################################################
# Import standard Python 2.7 modules.
from __future__ import print_function
import os, sys, time, argparse
# This module requires a pySerial installation.
# Package details: https://pypi.python.org/pypi/pyserial,
# Documentation: http://pythonhosted.org/pyserial/
import serial
################################################################
[docs]def issue_command(port, flow, extend=1, retract=2, verbose=False):
"""Issue an open-loop set of flow commands for a valve pair attached to an
actuator. Returns immediately. No return value.
:param port: serial port stream handle
:param flow: net flow from -100 full retraction rate to 100 full extension rate
:param extend: channel number for extension valve pair
:param retract: channel number for retract valve pair
:param verbose: flag to enable printed debugging output
:return: None
"""
# Make sure the flow value is integral and bounded.
flow = int(max(min(flow, 100),-100))
if flow > 0:
ext_command = "speed %d %d" % (extend, flow)
ret_command = "speed %d %d" % (retract, -100)
elif flow == 0:
ext_command = "speed %d %d" % (extend, 0)
ret_command = "speed %d %d" % (retract, 0)
else:
ext_command = "speed %d %d" % (extend, -100)
ret_command = "speed %d %d" % (retract, -flow)
if verbose:
print ("issuing %s, %s" % (ext_command, ret_command))
port.write("%s\n%s\n" % (ext_command, ret_command))
return
################################################################
[docs]def feedback_cycle(port, target, gain=0.5, **kwargs):
"""Run one cycle of proportional feedback control for a sensor and valve pair
attached to an actuator. Returns immediately or after an optional delay.
No return value. Extra keyword arguments are passed along to issue_command().
:param port: serial port stream handle
:param target: goal position, specified as an integer in arbitrary sensor units
:param gain: proportional gain scaling sensor units to actuator units
:param verbose: flag to enable printed debugging output
:return: None
"""
# Wait for a line of sensor data to be received from the Arduino.
raw_sensor_data = port.readline()
# The data format is specified in ValveControl, and may need to be adjusted if the sketch changes.
# This assumes it is a space-delimited set of six numbers:
# <raw-position-0> <calibrated-position-0> <pwm-output-0> <raw-position-1> <calibrated-position-1> <pwm-output-1>
# Divide the line into a list of strings.
value_strings = raw_sensor_data.split()
# Parse just the first raw position as an integer. This is simply the
# unfiltered 10-bit value of analogRead() from the potentiometer.
position = int(value_strings[0])
# For simplicity, the target is assumed to be specified in the same units.
# In practice, this value should be calibrated to reflect a real-world
# measurement so that gains can be chosen independent of the sensor scale.
position_error = target - position
# A positive error is assumed to map to a positive flow, e.g., the target is more extended than the current position.
# The scaling factor maps the arbitrary sensor unit to flow percentage unit.
flow_command = int(gain*position_error)
# Optionally print a report.
if kwargs.get('verbose', False):
print("position: %d, target: %d, error: %d, " % (position, target, position_error), end='')
# Send the computed command back to the valve controller.
issue_command(port, flow_command, **kwargs)
return
################################################################
# The following section is run when this is loaded as a script.
if __name__ == "__main__":
# Initialize the command parser.
parser = argparse.ArgumentParser(description = """Position regulation example using the ValveControl controller on an Arduino.""")
parser.add_argument('-v', '--verbose', action='store_true', help='Enable more detailed output.' )
parser.add_argument('--debug', action='store_true', help='Enable debugging output.' )
parser.add_argument('-p1', default=500, type=int, help='First position target.')
parser.add_argument('-p2', default=600, type=int, help='Second position target.')
parser.add_argument('-p', '--port', default='/dev/tty.usbmodem1411', help='Specify the name of the Arduino serial port device (default is /dev/tty.usbmodem1411).')
# Parse the command line, returning a Namespace.
args = parser.parse_args()
# Open the serial port, which should also reset the Arduino
print("Connecting to Arduino.")
port = serial.Serial(args.port, 115200, timeout=5 )
if args.verbose:
print("Opened serial port named", port.name)
print("Sleeping briefly while Arduino boots...")
time.sleep(2.0)
# throw away any extraneous input
print("Flushing Arduino input...")
port.flushInput()
# Begin the motion sequence. This may be safely interrupted by the user pressing Control-C.
kwargs = { 'verbose': args.verbose }
try:
print("Beginning movement sequence.")
for i in range(100):
feedback_cycle(port, target = args.p1, **kwargs)
for i in range(100):
feedback_cycle(port, target = args.p2, **kwargs)
for i in range(100):
feedback_cycle(port, target = args.p1, **kwargs)
for i in range(100):
feedback_cycle(port, target = args.p2, **kwargs)
print("Movement complete.")
except KeyboardInterrupt:
print("User interrupted motion.")
# Close the serial port connection.
port.write("stop\n")
port.close()