def __init__(self, parent=None):
super(AutostepProxyApp, self).__init__(parent)
self.setupUi(self)
self.initialize_ui()
self.tracking_mode_disable_list = [
self.currentPositionGroupBox,
self.setToGroupBox,
self.drivePowerGroupBox,
self.jogPositionGroupBox,
self.moveToPositionGroupBox,
self.runGroupBox,
self.stopGroupBox,
self.runGroupBox,
self.stopGroupBox,
]
self.busy_wait_disable_list = [
self.setToGroupBox,
self.drivePowerGroupBox,
self.jogPositionGroupBox,
self.moveToPositionGroupBox,
self.runGroupBox,
self.trackingModeGroupBox,
]
self.autostep_proxy = AutostepProxy()
self.enable_drive_power()
self.drivePowerCheckBox.setChecked(True)
self.set_tracking_mode(False)
self.trackingModeCheckBox.setChecked(False)
self.positionPollCheckBox.setChecked(True)
self.running = False
def __init__(self):
self.current_trial = DummyTrial()
self.current_trial_index = None
rospy.init_node('virtual_desert')
self.get_param()
self.rate = rospy.Rate(self.param['update_rate'])
self.lock = threading.Lock()
self.start_time = rospy.get_time()
self.angle_lowpass_filter = lowpass_filter.LowpassFilter(
self.param['angle_lowpass_fcut'])
self.angle_accumulator = angle_utils.AngleAccumulator()
self.angle_fixer = angle_utils.AngleFixer()
self.devices = {}
self.devices['panels_controller'] = display_ctrl.LedController()
self.devices['alicat_proxy'] = AlicatProxy()
self.devices['autostep_proxy'] = AutostepProxy()
self.devices['autostep_tracking_data_pub'] = rospy.Publisher(
'/autostep/tracking_data', TrackingData, queue_size=10)
self.devices['sunled_proxy'] = BasicLedStripProxy()
self.initialize_panels_controller()
self.initialize_autostep()
self.rolling_circ_mean = RollingCircularMean(
self.param['rolling_mean_size'])
self.angle_data_sub = rospy.Subscriber('/angle_data', MsgAngleData,
self.on_angle_data_callback)
self.data_pub = rospy.Publisher('/virtual_desert_data',
VirtualDesertData,
queue_size=10)
self.param_pub = rospy.Publisher('/virtual_desert_param',
std_msgs.msg.String,
queue_size=10)
#!/usr/bin/env python
from __future__ import print_function
import time
import scipy
from autostep_proxy import AutostepProxy
import matplotlib.pyplot as plt
# Create trajectory
dt = AutostepProxy.TrajectoryDt
num_cycle = 4
period = 3.0
num_pts = int(period * num_cycle / dt)
t = dt * scipy.arange(num_pts)
p = 80 * scipy.cos(2.0 * scipy.pi * t / period) - 50 * scipy.cos(
4.0 * scipy.pi * t / period)
plt.plot(t, p)
plt.xlabel('t (sec)')
plt.ylabel('position')
plt.grid('on')
plt.show()
# Run Trajectory
autostep = AutostepProxy()
autostep.run_trajectory(p)
while autostep.is_busy():
print('running')
time.sleep(0.1)
print('done')
from std_msgs.msg import Float64
from autostep_proxy import AutostepProxy
# Initialize argument parser
parser = argparse.ArgumentParser(description='Stepwise Trajectory')
parser.add_argument('-t', '--traj', metavar='TRAJECTORY', type=float, nargs='+',
help='List of trajectory parameters; format = t f [t f [...')
parser.add_argument('-r', '--rng', metavar='RANGE', type=float, nargs='+',
help='Frequency range parameters; format = t [start] stop [step]')
parser.add_argument('-p', '--pre', metavar='PREVIEW', type=bool,
help='Boolean checking whether to preview trajectory; default = True')
args = parser.parse_args()
autostep = AutostepProxy()
print()
print('* trajectory example')
print()
jog_params = {'speed': 200, 'accel': 500, 'decel': 500}
max_params = {'speed': 1000, 'accel': 10000, 'decel': 10000}
assert bool(args.traj) ^ bool(args.rng), "Arguments must be TRAJECTORY or LOOP but not both"
# Read trajectory params from command line or use default
if args.traj:
assert len(args.traj) % 2 == 0
freq_list = [args.traj[i:i+2] for i in range(0, len(args.traj), 2)]
elif args.rng:
#!/usr/bin/env python
from __future__ import print_function
import sys
import time
import scipy
import rospy
import std_msgs.msg
from autostep_proxy import AutostepProxy
from autostep_ros.msg import TrackingData
autostep = AutostepProxy()
# -----------------------------------------------------------------------------
if False:
print()
print('* testing run command')
autostep.set_move_mode('jog')
for vel in scipy.linspace(-200, 200, 11):
print(' vel: {}'.format(vel))
autostep.run(vel)
time.sleep(0.5)
autostep.run(0.0)
print()
# -----------------------------------------------------------------------------
if False:
print('* testing enable/release commands')
dt = 2.0
print(' releasing for {} secs ...'.format(dt), end='')
sys.stdout.flush()
#!/usr/bin/env python
from __future__ import print_function
import time
from autostep_proxy import AutostepProxy
print()
print('* sinusoid example')
print()
jog_params = {'speed': 200, 'accel': 500, 'decel': 500}
max_params = {'speed': 1000, 'accel': 10000, 'decel': 10000}
amplitude_list = [40, 80, 100]
autostep = AutostepProxy()
autostep.set_jog_mode_params(jog_params)
print(' jog params = {}'.format(autostep.get_jog_mode_params()))
autostep.set_max_mode_params(max_params)
print(' max params = {}'.format(autostep.get_max_mode_params()))
print()
for amplitude in amplitude_list:
print(' amplitude = {}'.format(amplitude))
param = {
'amplitude': amplitude,
'period': 2.5,
'phase': 90.0,
'offset': 0.0,
'num_cycle': 4
}
class AutostepProxyApp(QtWidgets.QMainWindow, Ui_MainWindow):
def __init__(self, parent=None):
super(AutostepProxyApp, self).__init__(parent)
self.setupUi(self)
self.initialize_ui()
self.tracking_mode_disable_list = [
self.currentPositionGroupBox,
self.setToGroupBox,
self.drivePowerGroupBox,
self.jogPositionGroupBox,
self.moveToPositionGroupBox,
self.runGroupBox,
self.stopGroupBox,
self.runGroupBox,
self.stopGroupBox,
]
self.busy_wait_disable_list = [
self.setToGroupBox,
self.drivePowerGroupBox,
self.jogPositionGroupBox,
self.moveToPositionGroupBox,
self.runGroupBox,
self.trackingModeGroupBox,
]
self.autostep_proxy = AutostepProxy()
self.enable_drive_power()
self.drivePowerCheckBox.setChecked(True)
self.set_tracking_mode(False)
self.trackingModeCheckBox.setChecked(False)
self.positionPollCheckBox.setChecked(True)
self.running = False
def initialize_ui(self):
self.set_lcd_colors()
self.timer = QtCore.QTimer()
self.timer_period = 100
self.timer.start(self.timer_period)
self.setToLineEdit.setText(str(0))
self.setToLineEditValidator = QtGui.QDoubleValidator()
self.setToLineEdit.setValidator(self.setToLineEditValidator)
self.jogStepLineEdit.setText(str(10))
self.jogStepLineEditValidator = QtGui.QDoubleValidator()
self.jogStepLineEdit.setValidator(self.jogStepLineEditValidator)
self.moveToLineEdit.setText(str(0))
self.moveToLineEditValidator = QtGui.QDoubleValidator()
self.moveToLineEdit.setValidator(self.moveToLineEditValidator)
self.runLineEdit.setText(str(50))
self.runLineEditValidator = QtGui.QDoubleValidator()
self.runLineEdit.setValidator(self.runLineEditValidator)
self.connect_ui_actions()
def set_lcd_colors(self):
palette = self.currentPositionLcdNumber.palette()
palette.setColor(palette.WindowText, QtGui.QColor(0, 200, 255))
palette.setColor(palette.Light, QtGui.QColor(0, 0, 0))
palette.setColor(palette.Dark, QtGui.QColor(0, 0, 0))
palette.setColor(palette.Background, QtGui.QColor(0, 0, 0))
self.currentPositionLcdNumber.setPalette(palette)
def connect_ui_actions(self):
self.timer.timeout.connect(self.on_timer)
self.setToPushButton.clicked.connect(self.on_setTo_button_clicked)
self.jogPosPushButton.clicked.connect(self.on_jog_pos_button_clicked)
self.jogNegPushButton.clicked.connect(self.on_jog_neg_button_clicked)
self.moveToPushButton.clicked.connect(self.on_move_to_button_clicked)
self.drivePowerCheckBox.stateChanged.connect(
self.on_drive_power_changed)
self.trackingModeCheckBox.stateChanged.connect(
self.on_tracking_mode_changed)
self.stopPushButton.clicked.connect(self.on_stop_button_clicked)
self.runPushButton.clicked.connect(self.on_run_button_clicked)
def on_setTo_button_clicked(self):
value = float(self.setToLineEdit.text())
self.set_position(value)
def on_jog_pos_button_clicked(self):
value = float(self.jogStepLineEdit.text())
self.jog_position(value)
def on_jog_neg_button_clicked(self):
value = -float(self.jogStepLineEdit.text())
self.jog_position(value)
def on_move_to_button_clicked(self):
value = float(self.moveToLineEdit.text())
self.move_to_position(value)
def on_drive_power_changed(self, value):
if value == QtCore.Qt.Checked:
self.enable_drive_power()
else:
self.disable_drive_power()
def on_tracking_mode_changed(self, value):
if value == QtCore.Qt.Checked:
self.tracking_mode_enabled = True
self.autostep_proxy.enable_tracking_mode()
else:
self.autostep_proxy.disable_tracking_mode()
self.tracking_mode_enabled = False
self.set_widget_enabled_for_tracking_mode()
def on_stop_button_clicked(self):
self.autostep_proxy.soft_stop()
self.running = False
def on_run_button_clicked(self):
value = float(self.runLineEdit.text())
self.running = True
self.autostep_proxy.run(value)
def on_timer(self):
position_str = ''
if self.position_poll_enabled():
position = self.autostep_proxy.get_position()
if position is not None:
position = round(position, 1)
position_str = '{:0.1f}'.format(position)
self.currentPositionLcdNumber.display(position_str)
proxy_is_busy = self.autostep_proxy.is_busy()
self.set_widget_enabled_for_busy(proxy_is_busy or self.running)
def set_widget_enabled_for_tracking_mode(self):
if self.tracking_mode_enabled:
for widget in self.tracking_mode_disable_list:
self.timer.stop()
self.currentPositionLcdNumber.display('')
widget.setEnabled(False)
else:
for widget in self.tracking_mode_disable_list:
widget.setEnabled(True)
self.timer.start(self.timer_period)
def enable_drive_power(self):
self.autostep_proxy.enable()
def disable_drive_power(self):
self.autostep_proxy.release()
def set_position(self, value):
self.autostep_proxy.set_position(value)
def move_to_position(self, value):
self.autostep_proxy.move_to(value)
def jog_position(self, value):
self.autostep_proxy.set_move_mode('jog')
self.autostep_proxy.move_by(value)
def set_tracking_mode(self, value):
if value:
self.autostep_proxy.enable_tracking_mode()
self.tracking_mode_enabled = True
else:
self.autostep_proxy.disable_tracking_mode()
self.tracking_mode_enabled = False
def set_widget_enabled_for_busy(self, is_busy):
if is_busy:
for widget in self.busy_wait_disable_list:
widget.setEnabled(False)
else:
for widget in self.busy_wait_disable_list:
widget.setEnabled(True)
def position_poll_enabled(self):
return self.positionPollCheckBox.isChecked()
#!/usr/bin/env python
from __future__ import print_function
import sys
import time
import scipy
import rospy
import std_msgs.msg
from autostep_proxy import AutostepProxy
from autostep_ros.msg import TrackingData
autostep = AutostepProxy()
cmd = sys.argv[1]
if cmd.lower() == 'true':
print('* testing enable/disable tracking mode')
print(' enabling')
autostep.set_position(0.0)
autostep.enable_tracking_mode()
else:
print(' disabling')
autostep.disable_tracking_mode()
from __future__ import print_function
import time
from autostep_proxy import AutostepProxy
num_cycle = 5
velocity = 600
jog_params = {'speed': 200, 'accel': 500, 'decel': 500}
max_params = {'speed': 1000, 'accel': 2000, 'decel': 2000}
print()
print('* run at velocity example')
print()
autostep = AutostepProxy()
autostep.set_jog_mode_params(jog_params)
print(' jog params = {}'.format(autostep.get_jog_mode_params()))
autostep.set_max_mode_params(max_params)
print(' max params = {}'.format(autostep.get_max_mode_params()))
print()
print(' move to position 0.0')
autostep.set_move_mode('jog')
autostep.move_to(0.0)
autostep.busy_wait()
sleep_dt = 2.0
print(' sleeping for {0} sec'.format(sleep_dt))