def test_servo1(): useq = [0.5, 1, 1.5, 1, 0.5 ,0, -0.5 , -1, -1.5 , -0.5,0 ,0.5, 1, 1.5, 1, 0.5 ,0, -0.5 , -1, -1.5 , -0.5,0] """Control motors directly with sliders.""" slb = gui.createSliderBox(robot.dimU,'Direct Motor Control') for i in range(0,robot.dimU): slb.setupSlider(i,robot.u_llim[i],robot.u_ulim[i],'u%d'%i,0.01) y=robot.read(); qdot = 0; q = y[0]; qddot = y[1] # read arm state slb.setValues([0,0,0]) # set sliders to zeros # scope for plotting out sensor signals q_scope = gui.Scope(1, 0,250,600,200,'Position') qdot_scope = gui.Scope(1, 0,475,600,200,'Velocity') qddot_scope = gui.Scope(1, 0,700,600,200,'Acceleration') m0_scope = gui.Scope(2,680,250,600,200,'Motor 0 Commanded/Actual Positions') m1_scope = gui.Scope(2,680,475,600,200,'Motor 1 Commanded/Actual Positions') m = [0,0] for n in range(0,22): u = slb.getValues() u[0] = useq[n] y = robot.run_step(u) qdot = (y[0]-q)/0.02; q = y[0]; qddot = y[1]; m[0]=y[2]; m[1]=y[3]; q_scope .add([q]) qdot_scope .add([qdot]) qddot_scope.add([qddot]) m0_scope .add([u[0],m[0]]) m1_scope .add([u[1],m[1]]) time.sleep(1) # return to zero position when done go_zeros()
def emg_control():
"""Control motors directly with EMG."""
slb = gui.createSliderBox(10, 'EMG Eq. Pos./Stiffness Control')
slb.setupSlider(0, 0, 2000, 'EMG High Pass', 0.1)
slb.setupSlider(1, 0, 2000, 'EMG Low Pass', 0.1)
slb.setupSlider(2, 0, 10, 'EMG Smoothing', 0.01)
slb.setupSlider(3, 1, 10, 'EMG0 gain (red)', 0.1)
slb.setupSlider(4, -1, 10, 'EMG0 offset (red)', 0.0001)
slb.setupSlider(5, 1, 10, 'EMG1 gain (blue)', 0.1)
slb.setupSlider(6, -1, 10, 'EMG1 offset (blue)', 0.0001)
slb.setupSlider(7, 0, 50, 'u0 gain', 0.1)
slb.setupSlider(8, 0, 50, 'u1 gain', 0.1)
slb.setupSlider(9, -1, 1, 'Normal / Reverse', 1)
slb.setValues([60, 600, 2.24, 2.1, 0, 2.9, 0, 0, 0, 1])
emg = audio.AudioInterface()
emg_scope = gui.Scope(2, 620, 0, 600, 200, 'EMG Signals')
u0_scope = gui.Scope(2, 620, 250, 600, 200,
'Desired/actual equilibrium position')
u1_scope = gui.Scope(2, 620, 475, 600, 200, 'Desired/actual stiffness')
u1min = .01
a = zeros([2, 1])
u = zeros([robot.dimU, 1])
ud = zeros([robot.dimU, 1])
while slb.checkExit() == 0:
v = slb.getValues()
emg.setHP(v[0])
emg.setLP(v[1])
emg.setSmooth(v[2])
a = list(emg.getData())
a[0] = v[3] * a[0] + v[4]
a[1] = v[5] * a[1] + v[6]
if len(a) > 0:
emg_scope.add(a)
# estimate desired eq. pos and stiffness
ud[0] = v[9] * v[7] * (
a[0] - a[1]) # u0 (= q0) -> scaled difference between signals
ud[1] = v[8] * ((a[0] + a[1] - math.fabs(a[0] - a[1])) /
2) + u1min # u1 (= k) -> min of the two signals
u = robot.clip_commands(ud)
y = robot.run_step(u)
u0_scope.add([ud[0], u[0]])
u1_scope.add([ud[1], u[1]])
def online_plot():
# scope for plotting out sensor signals
q_scope = gui.Scope(1, 0, 250, 600, 200, 'Position')
qdot_scope = gui.Scope(1, 0, 475, 600, 200, 'Velocity')
qddot_scope = gui.Scope(1, 0, 700, 600, 200, 'Acceleration')
m1_scope = gui.Scope(2, 680, 250, 600, 200,
'Motor 1 Commanded/Actual Positions')
m2_scope = gui.Scope(2, 680, 475, 600, 200,
'Motor 2 Commanded/Actual Positions')
m = [0, 0]
while q_scope.checkExit() == 0:
#u = slb.getValues()
y = robot.read()
qdot = (y[0] - q) / 0.02
q = y[0]
qddot = y[1]
m[0] = y[2]
m[1] = y[3]
q_scope.add([q])
qdot_scope.add([qdot])
qddot_scope.add([qddot])
m0_scope.add([u[0], m[0]])
m1_scope.add([u[1], m[1]])
def direct_motor_control():
"""Control motors directly with sliders."""
slb = gui.createSliderBox(robot.dimU, 'Direct Motor Control')
for i in range(0, robot.dimU):
slb.setupSlider(i, robot.u_llim[i], robot.u_ulim[i], 'u%d' % i, 0.01)
y = robot.read()
qdot = 0
q = y[0]
qddot = y[1] # read arm state
slb.setValues([0, 0, 0]) # set sliders to zeros
# scope for plotting out sensor signals
q_scope = gui.Scope(1, 0, 250, 600, 200, 'Position')
qdot_scope = gui.Scope(1, 0, 475, 600, 200, 'Velocity')
qddot_scope = gui.Scope(1, 0, 700, 600, 200, 'Acceleration')
m0_scope = gui.Scope(2, 680, 250, 600, 200,
'Motor 0 Commanded/Actual Positions')
m1_scope = gui.Scope(2, 680, 475, 600, 200,
'Motor 1 Commanded/Actual Positions')
m = [0, 0]
while slb.checkExit() == 0:
u = slb.getValues()
y = robot.run_step(u)
qdot = (y[0] - q) / 0.02
q = y[0]
qddot = y[1]
m[0] = y[2]
m[1] = y[3]
q_scope.add([q])
qdot_scope.add([qdot])
qddot_scope.add([qddot])
m0_scope.add([u[0], m[0]])
m1_scope.add([u[1], m[1]])
# return to zero position when done
go_zeros()
## \file test_audio.py # \author Matthew Howard (MH), [email protected] # \brief Script for testing soundcard data acquisition # \ingroup Audio import time import math from numpy import * import pyrex_gui as gui import pyrex_audio as audio # Time constant (ms) dt = 0.002 # Window to plot signals s = gui.Scope(2, 0, 0, 800, 400, 'Signals') # Sliders to adjust filters/smoothing. slb = gui.createSliderBox(3, 'Filters') slb.setupSlider(0, 0, 2000, 'High Pass', 0.1) slb.setupSlider(1, 0, 2000, 'Low Pass', 0.1) slb.setupSlider(2, 0, 100, 'Smoothing', 0.1) slb.setValues([60, 600, 3]) # Audio object a = audio.AudioInterface() def show_signal(): """ Show signals. """ while slb.checkExit() == 0: