def __init__(self, ipcon, uid, version):
PluginBase.__init__(self, ipcon, uid, 'Servo Brick', version)
self.setupUi(self)
self.servo = BrickServo(uid, ipcon)
self.device = self.servo
self.position_list = []
self.velocity_list = []
self.acceleration_list = []
self.enable_list = []
self.up_cur = 0
self.up_siv = 0
self.up_eiv = 0
self.up_opv = 0
self.up_mv = 0
self.up_ena = [0]*7
self.up_pos = [0]*7
self.up_vel = [0]*7
self.up_acc = [0]*7
self.update_timer = QTimer()
self.update_timer.timeout.connect(self.update_apply)
self.update_timer.setInterval(50)
self.update_timer.start()
self.alive = True
for i in range(1, 8):
label = QLabel()
label.setText('Off')
self.enable_list.append(label)
self.status_grid.addWidget(label, i, 1)
for i in range(1, 8):
pk = PositionKnob()
self.position_list.append(pk)
self.status_grid.addWidget(pk, i, 2)
for i in range(1, 8):
cb = ColorBar(Qt.Vertical)
self.velocity_list.append(cb)
self.status_grid.addWidget(cb, i, 3)
for i in range(1, 8):
cb = ColorBar(Qt.Vertical)
self.acceleration_list.append(cb)
self.status_grid.addWidget(cb, i, 4)
self.qem = QErrorMessage(self)
self.qem.setWindowTitle("Under Voltage")
self.test_button.clicked.connect(self.test_button_clicked)
self.output_voltage_button.clicked.connect(self.output_voltage_button_clicked)
self.servo_dropbox.currentIndexChanged.connect(lambda x: self.update_servo_specific())
self.enable_checkbox.stateChanged.connect(self.enable_state_changed)
self.position_slider.sliderReleased.connect(self.position_slider_released)
self.position_slider.valueChanged.connect(self.position_spin.setValue)
self.position_spin.editingFinished.connect(self.position_spin_finished)
self.velocity_slider.sliderReleased.connect(self.velocity_slider_released)
self.velocity_slider.valueChanged.connect(self.velocity_spin.setValue)
self.velocity_spin.editingFinished.connect(self.velocity_spin_finished)
self.acceleration_slider.sliderReleased.connect(self.acceleration_slider_released)
self.acceleration_slider.valueChanged.connect(self.acceleration_spin.setValue)
self.acceleration_spin.editingFinished.connect(self.acceleration_spin_finished)
self.period_slider.sliderReleased.connect(self.period_slider_released)
self.period_slider.valueChanged.connect(self.period_spin.setValue)
self.period_spin.editingFinished.connect(self.period_spin_finished)
self.pulse_width_min_spin.editingFinished.connect(self.pulse_width_spin_finished)
self.pulse_width_max_spin.editingFinished.connect(self.pulse_width_spin_finished)
self.degree_min_spin.editingFinished.connect(self.degree_spin_finished)
self.degree_max_spin.editingFinished.connect(self.degree_spin_finished)
self.minimum_voltage_button.pressed.connect(self.minimum_voltage_button_pressed)
self.qtcb_under_voltage.connect(self.cb_under_voltage)
self.servo.register_callback(self.servo.CALLBACK_UNDER_VOLTAGE,
self.qtcb_under_voltage.emit)
class WorkerThread(QThread):
def __init__(self, parent = None, func = None):
QThread.__init__(self, parent)
self.func = func
def run(self):
self.func()
self.test_event = Event()
self.test_thread_object = WorkerThread(self, self.test_thread)
self.update_event = Event()
self.update_thread_object = WorkerThread(self, self.update_thread)
self.update_thread_first_time = False
self.update_done_event = Event()
self.update_done_event.set()
class Servo(PluginBase, Ui_Servo):
qtcb_under_voltage = pyqtSignal(int)
def __init__(self, ipcon, uid, version):
PluginBase.__init__(self, ipcon, uid, 'Servo Brick', version)
self.setupUi(self)
self.servo = BrickServo(uid, ipcon)
self.device = self.servo
self.position_list = []
self.velocity_list = []
self.acceleration_list = []
self.enable_list = []
self.up_cur = 0
self.up_siv = 0
self.up_eiv = 0
self.up_opv = 0
self.up_mv = 0
self.up_ena = [0]*7
self.up_pos = [0]*7
self.up_vel = [0]*7
self.up_acc = [0]*7
self.update_timer = QTimer()
self.update_timer.timeout.connect(self.update_apply)
self.update_timer.setInterval(50)
self.update_timer.start()
self.alive = True
for i in range(1, 8):
label = QLabel()
label.setText('Off')
self.enable_list.append(label)
self.status_grid.addWidget(label, i, 1)
for i in range(1, 8):
pk = PositionKnob()
self.position_list.append(pk)
self.status_grid.addWidget(pk, i, 2)
for i in range(1, 8):
cb = ColorBar(Qt.Vertical)
self.velocity_list.append(cb)
self.status_grid.addWidget(cb, i, 3)
for i in range(1, 8):
cb = ColorBar(Qt.Vertical)
self.acceleration_list.append(cb)
self.status_grid.addWidget(cb, i, 4)
self.qem = QErrorMessage(self)
self.qem.setWindowTitle("Under Voltage")
self.test_button.clicked.connect(self.test_button_clicked)
self.output_voltage_button.clicked.connect(self.output_voltage_button_clicked)
self.servo_dropbox.currentIndexChanged.connect(lambda x: self.update_servo_specific())
self.enable_checkbox.stateChanged.connect(self.enable_state_changed)
self.position_slider.sliderReleased.connect(self.position_slider_released)
self.position_slider.valueChanged.connect(self.position_spin.setValue)
self.position_spin.editingFinished.connect(self.position_spin_finished)
self.velocity_slider.sliderReleased.connect(self.velocity_slider_released)
self.velocity_slider.valueChanged.connect(self.velocity_spin.setValue)
self.velocity_spin.editingFinished.connect(self.velocity_spin_finished)
self.acceleration_slider.sliderReleased.connect(self.acceleration_slider_released)
self.acceleration_slider.valueChanged.connect(self.acceleration_spin.setValue)
self.acceleration_spin.editingFinished.connect(self.acceleration_spin_finished)
self.period_slider.sliderReleased.connect(self.period_slider_released)
self.period_slider.valueChanged.connect(self.period_spin.setValue)
self.period_spin.editingFinished.connect(self.period_spin_finished)
self.pulse_width_min_spin.editingFinished.connect(self.pulse_width_spin_finished)
self.pulse_width_max_spin.editingFinished.connect(self.pulse_width_spin_finished)
self.degree_min_spin.editingFinished.connect(self.degree_spin_finished)
self.degree_max_spin.editingFinished.connect(self.degree_spin_finished)
self.minimum_voltage_button.pressed.connect(self.minimum_voltage_button_pressed)
self.qtcb_under_voltage.connect(self.cb_under_voltage)
self.servo.register_callback(self.servo.CALLBACK_UNDER_VOLTAGE,
self.qtcb_under_voltage.emit)
class WorkerThread(QThread):
def __init__(self, parent = None, func = None):
QThread.__init__(self, parent)
self.func = func
def run(self):
self.func()
self.test_event = Event()
self.test_thread_object = WorkerThread(self, self.test_thread)
self.update_event = Event()
self.update_thread_object = WorkerThread(self, self.update_thread)
self.update_thread_first_time = False
self.update_done_event = Event()
self.update_done_event.set()
def stop(self):
if self.test_button.text() == "Stop Test":
self.test_button_clicked()
self.update_event.clear()
self.alive = False
self.update_done_event.wait()
def start(self):
self.alive = True
self.test_event.clear()
self.update_done_event.set()
self.update_event.set()
if not self.test_thread_object.isRunning():
self.test_thread_object.start()
if not self.update_thread_object.isRunning():
self.update_thread_object.start()
self.update_servo_specific()
def destroy(self):
self.test_event.set()
self.update_event.set()
def has_reset_device(self):
return self.version >= (1, 1, 3)
def reset_device(self):
if self.has_reset_device():
self.servo.reset()
def is_brick(self):
return True
def get_url_part(self):
return 'servo'
@staticmethod
def has_device_identifier(device_identifier):
return device_identifier == BrickServo.DEVICE_IDENTIFIER
def get_period_async(self, per):
self.period_spin.setValue(per)
self.period_slider.setValue(per)
def is_enabled_async(self, ena):
if ena:
self.enable_checkbox.setCheckState(Qt.Checked)
else:
self.enable_checkbox.setCheckState(Qt.Unchecked)
def get_position_async(self, pos):
self.position_spin.setValue(pos)
self.position_slider.setValue(pos)
def get_velocity_async(self, vel):
self.velocity_spin.setValue(vel)
self.velocity_slider.setValue(vel)
def get_acceleration_async(self, acc):
self.acceleration_spin.setValue(acc)
self.acceleration_slider.setValue(acc)
def get_degree_async(self, deg, i):
deg_min, deg_max = deg
self.degree_min_spin.setValue(deg_min)
self.degree_max_spin.setValue(deg_max)
self.position_slider.setMinimum(deg_min)
self.position_slider.setMaximum(deg_max)
self.position_spin.setMinimum(deg_min)
self.position_spin.setMaximum(deg_max)
self.position_list[i].setTotalAngle((deg_max - deg_min)/100)
self.position_list[i].setRange(deg_min/100, deg_max/100)
def get_pulse_width_async(self, pulse):
pulse_min, pulse_max = pulse
self.pulse_width_min_spin.setValue(pulse_min)
self.pulse_width_max_spin.setValue(pulse_max)
def update_servo_specific(self):
i = self.selected_servo()
if i == 255:
self.enable_checkbox.setCheckState(Qt.Unchecked)
return
async_call(self.servo.get_position, i, self.get_position_async, self.increase_error_count)
async_call(self.servo.get_velocity, i, self.get_velocity_async, self.increase_error_count)
async_call(self.servo.get_acceleration, i, self.get_acceleration_async, self.increase_error_count)
async_call(self.servo.get_period, i, self.get_period_async, self.increase_error_count)
async_call(self.servo.is_enabled, i, self.is_enabled_async, self.increase_error_count)
def get_lambda_deg(i):
return lambda x: self.get_degree_async(x, i)
async_call(self.servo.get_degree, i, get_lambda_deg(i), self.increase_error_count)
async_call(self.servo.get_pulse_width, i, self.get_pulse_width_async, self.increase_error_count)
def error_handler(self, error):
pass
def servo_current_update(self, value):
self.current_label.setText(str(value) + "mA")
def stack_input_voltage_update(self, sv):
sv_str = "%gV" % round(sv/1000.0, 1)
self.stack_voltage_label.setText(sv_str)
def external_input_voltage_update(self, ev):
ev_str = "%gV" % round(ev/1000.0, 1)
self.external_voltage_label.setText(ev_str)
def output_voltage_update(self, ov):
ov_str = "%gV" % round(ov/1000.0, 1)
self.output_voltage_label.setText(ov_str)
def minimum_voltage_update(self, mv):
mv_str = "%gV" % round(mv/1000.0, 1)
self.minimum_voltage_label.setText(mv_str)
def position_update(self, i, pos):
self.position_list[i].setValue(pos/100)
def velocity_update(self, i, vel):
self.velocity_list[i].set_height(vel*100/0xFFFF)
def acceleration_update(self, i, acc):
self.acceleration_list[i].set_height(acc*100/0xFFFF)
def deactivate_servo(self, i):
if self.enable_list[i].text() != 'Off':
self.velocity_list[i].grey()
self.acceleration_list[i].grey()
def activate_servo(self, i):
if self.enable_list[i].text() != 'On':
self.velocity_list[i].color()
self.acceleration_list[i].color()
def selected_servo(self):
try:
return int(self.servo_dropbox.currentText()[-1:])
except:
return 255
def enable_state_changed(self, state):
s = self.selected_servo()
try:
if state == Qt.Checked:
self.servo.enable(s)
elif state == Qt.Unchecked:
self.servo.disable(s)
except ip_connection.Error:
return
def update_apply(self):
if not self.update_thread_first_time:
return
self.servo_current_update(self.up_cur)
self.stack_input_voltage_update(self.up_siv)
self.external_input_voltage_update(self.up_eiv)
self.output_voltage_update(self.up_opv)
self.minimum_voltage_update(self.up_mv)
for i in range(7):
if self.up_ena[i]:
self.activate_servo(i)
self.position_update(i, self.up_pos[i])
self.velocity_update(i, self.up_vel[i])
self.acceleration_update(i, self.up_acc[i])
self.enable_list[i].setText('On')
else:
self.deactivate_servo(i)
self.enable_list[i].setText('Off')
def update_thread(self):
while self.alive:
time.sleep(0.1)
try:
servo = self.selected_servo()
if servo == 255:
self.up_cur = self.servo.get_overall_current()
else:
self.up_cur = self.servo.get_servo_current(servo)
self.up_siv = self.servo.get_stack_input_voltage()
self.up_eiv = self.servo.get_external_input_voltage()
self.up_opv = self.servo.get_output_voltage()
self.up_mv = self.servo.get_minimum_voltage()
for i in range(7):
self.up_ena[i] = self.servo.is_enabled(i)
if self.up_ena[i]:
self.activate_servo(i)
self.up_pos[i] = self.servo.get_current_position(i)
self.up_vel[i] = self.servo.get_current_velocity(i)
self.up_acc[i] = self.servo.get_acceleration(i)
#self.update_apply()
self.update_done_event.set()
self.update_event.wait()
self.update_done_event.clear()
except ip_connection.Error:
pass
self.update_thread_first_time = True
self.update_done_event.set()
def test_thread(self):
def test(num, ena, acc, vel, pos):
if not self.alive:
return
try:
if ena:
self.servo.enable(num)
else:
self.servo.disable(num)
self.servo.set_acceleration(num, acc)
self.servo.set_velocity(num, vel)
self.servo.set_position(num, pos)
except ip_connection.Error:
return
def random_test():
for i in range(7):
test(i,
random.randrange(0, 2),
random.randrange(0, 65536),
random.randrange(0, 65536),
random.randrange(-9000, 9001))
while self.alive:
self.test_event.wait()
if not self.alive:
return
# Full Speed left for 2 seconds
for i in range(7):
test(i, True, 65535, 65535, 9000)
time.sleep(2)
self.test_event.wait()
if not self.alive:
return
# Full Speed right for 2 seconds
for i in range(7):
test(i, True, 65535, 65535, -9000)
time.sleep(2)
self.test_event.wait()
if not self.alive:
return
# Test different speeds
for i in range(19):
for j in range(7):
test(j, True, 65535, 65535*i/18, -9000+i*1000)
time.sleep(0.1)
self.test_event.wait()
if not self.alive:
return
time.sleep(1)
self.test_event.wait()
if not self.alive:
return
# Test acceleration
for i in range(7):
test(i, True, 100, 65535, -9000)
time.sleep(3)
self.test_event.wait()
if not self.alive:
return
# Random for 3 seconds
random_test()
time.sleep(3)
def test_button_clicked(self):
if self.test_button.text() == "Start Test":
self.test_button.setText("Stop Test")
self.test_event.set()
else:
self.test_button.setText("Start Test")
self.test_event.clear()
def output_voltage_button_clicked(self):
qid = QInputDialog(self)
qid.setInputMode(QInputDialog.IntInput)
qid.setIntMinimum(2000)
qid.setIntMaximum(9000)
qid.setIntStep(100)
async_call(self.servo.get_output_voltage, None, qid.setIntValue, self.increase_error_count)
qid.intValueSelected.connect(self.output_voltage_selected)
qid.setLabelText("Choose Output Voltage in mV.")
# "<font color=red>Setting this too high can destroy your servo.</font>")
qid.open()
def output_voltage_selected(self, value):
try:
self.servo.set_output_voltage(value)
except ip_connection.Error:
return
def position_slider_released(self):
value = self.position_slider.value()
self.position_spin.setValue(value)
try:
self.servo.set_position(self.selected_servo(), value)
except ip_connection.Error:
return
def position_spin_finished(self):
value = self.position_spin.value()
self.position_slider.setValue(value)
try:
self.servo.set_position(self.selected_servo(), value)
except ip_connection.Error:
return
def velocity_slider_released(self):
value = self.velocity_slider.value()
self.velocity_spin.setValue(value)
try:
self.servo.set_velocity(self.selected_servo(), value)
except ip_connection.Error:
return
def velocity_spin_finished(self):
value = self.velocity_spin.value()
self.velocity_slider.setValue(value)
try:
self.servo.set_velocity(self.selected_servo(), value)
except ip_connection.Error:
return
def acceleration_slider_released(self):
value = self.acceleration_slider.value()
self.acceleration_spin.setValue(value)
try:
self.servo.set_acceleration(self.selected_servo(), value)
except ip_connection.Error:
return
def acceleration_spin_finished(self):
value = self.acceleration_spin.value()
self.acceleration_slider.setValue(value)
try:
self.servo.set_acceleration(self.selected_servo(), value)
except ip_connection.Error:
return
def period_slider_released(self):
value = self.period_slider.value()
self.period_spin.setValue(value)
try:
self.servo.set_period(self.selected_servo(), value)
except ip_connection.Error:
return
def period_spin_finished(self):
value = self.period_spin.value()
self.period_slider.setValue(value)
try:
self.servo.set_period(self.selected_servo(), value)
except ip_connection.Error:
return
def pulse_width_spin_finished(self):
try:
self.servo.set_pulse_width(self.selected_servo(),
self.pulse_width_min_spin.value(),
self.pulse_width_max_spin.value())
except ip_connection.Error:
return
def degree_spin_finished(self):
min = self.degree_min_spin.value()
max = self.degree_max_spin.value()
servo = self.selected_servo()
self.position_slider.setMinimum(min)
self.position_slider.setMaximum(max)
self.position_spin.setMinimum(min)
self.position_spin.setMaximum(max)
if servo == 255:
for i in range(7):
self.position_list[i].setTotalAngle((max - min)/100)
self.position_list[i].setRange(min/100, max/100)
else:
self.position_list[servo].setTotalAngle((max - min)/100)
self.position_list[servo].setRange(min/100, max/100)
try:
self.servo.set_degree(servo, min, max)
except ip_connection.Error:
return
def cb_under_voltage(self, ov):
mv_str = self.minimum_voltage_label.text()
ov_str = "%gV" % round(ov/1000.0, 1)
if not self.qem.isVisible():
self.qem.showMessage("Under Voltage: Output Voltage of " + ov_str +
" is below minimum voltage of " + mv_str,
"Servo_UnterVoltage")
def minimum_voltage_selected(self, value):
try:
self.servo.set_minimum_voltage(value)
except ip_connection.Error:
return
def minimum_voltage_button_pressed(self):
qid = QInputDialog(self)
qid.setInputMode(QInputDialog.IntInput)
qid.setIntMinimum(5000)
qid.setIntMaximum(0xFFFF)
qid.setIntStep(100)
async_call(self.servo.get_minimum_voltage, None, qid.setIntValue, self.increase_error_count)
qid.intValueSelected.connect(self.minimum_voltage_selected)
qid.setLabelText("Choose minimum servo voltage in mV.")
qid.open()
