def init_variables(self):
''' Iniciação das variaveis da classe '''
self.scannedChannels = [1, 2, 3] # escolher canais nessa variavel
self.stimMsg = Stimulator()
self.stimMsg.channel = [1]
self.stimMsg.mode = ['single']
self.counter = 0
self.channelIndex = 0
self.maxAngle = []
self.breaker = False
self.channelCurrent = {}
for c in self.scannedChannels:
self.channelCurrent[c] = 0
self.angleThreshold = 30
self.min_current = 10
self.current_step = 2
self.current = self.min_current
def init_variables(self):
''' Iniciação das variaveis da classe '''
self.stimMsg = Stimulator()
self.stimMsg.mode = ['single']
self.channelCurrent = {}
self.channelCurrentReference = []
self.start = False
self.counter = 0
self.totalCounter = 0
self.activeChannels = 0
self.activeCurrents = 0
self.activePulseWidths = 0
self.maxAngle = 0
self.angleThreshold = 20
################################################## ######### Iniciando variáveis globais ############ ################################################## lowerRightLegAngle = -1 upperRightLegAngle = -1 rightKneeAngle = -1 lowerLeftLegAngle = -1 upperLeftLegAngle = -1 leftKneeAngle = -1 # estado atual state = state0 # lista com todos estados, para implementação de botões usando outra imu stateList = [state0, state1, state2, state3, state4, state5] stimMsg = Stimulator() #[quadrícepsDireito, ísquiosDireito, 'pé caído'Direito, panturrilhaDireito, quadrícepsEsquerdo, ísquiosEsquerdo, 'pé caído'Esquerdo, panturrilhaEsquerdo] # Nota: músculo do 'pé caído' ativa com facilidade, usar corrente baixa #stimMsg.channel = [1, 2, 3, 4, 5, 6, 7, 8] #stimMsg.mode = ['single', 'single', 'single', 'single', 'single', 'single', 'single', 'single'] ##stimMsg.pulse_current = [2, 2, 2, 2, 0, 0, 0, 0] # currenet in mA ##stimMsg.pulse_width = [0, 0, 0, 0, 0, 0, 0, 0] #[quadrícepsDireito, ísquiosDireito, quadrícepsEsquerdo, ísquiosEsquerdo] stimMsg.channel = [1, 2, 3, 4] stimMsg.mode = ['single', 'single', 'single', 'single'] stimMsg.pulse_current = [18, 18, 18, 16] # currenet in mA #stimMsg.pulse_current = [10, 10, 10, 10] # currenet in mA stimMsg.pulse_width = [0, 0, 0, 0]
rospy.sleep(5)
counter = 0
channel += 1
else:
breaker = True
##################################################
##### Iniciação de variáveis globais #############
##################################################
numOfChannels = 2
stimMsg = Stimulator()
stimMsg.channel = [1]
stimMsg.mode = ['single']
counter = 0
channel = 1
maxAngle = []
bestChannels = []
breaker = False
##################################################
##### Loop do ROS ################################
def control_knee():
global t_control
global stimMsg
global update_values
global co_activation
global new_current_quad
global new_current_hams
global control_sel
global pw_min_h
global pw_min_q
global channels_sel
global control_onoff
global control_enable_quad
global control_enable_hams
global ilc_memory
global ilc_i
global Kp_now
global Ki_now
global Kd_now
global ES_A_now
global ES_omega_now
global ES_phase_now
global ES_K_now
global ES_RC_now
global ILC_alpha_now
global ILC_beta_now
global ILC_gama_now
global initTime
# init_variables
t_control = [0, 0]
# init control node
controller = control.Control(rospy.init_node('control', anonymous=False))
# communicate with the dynamic server
dyn_params = reconfig.Client('server', config_callback=server_callback)
# subscribed topics
sub = dict()
sub.update({
'pedal':
rospy.Subscriber('imu/pedal', Imu, callback=imu_callback),
'ref':
rospy.Subscriber('ref_channel', Float64, callback=reference_callback),
'steps':
rospy.Subscriber('steps_channel', Float64, callback=steps_callback),
'curve':
rospy.Subscriber('calibration_channel',
Float64,
callback=curve_callback)
})
# sub = {}
# sub['pedal'] = rospy.Subscriber('imu/pedal', Imu, callback=imu_callback)
# sub['ref'] = rospy.Subscriber('ref_channel', Float64, callback=reference_callback)
# sub['steps'] = rospy.Subscriber('steps_channel', Float64, callback=steps_callback)
# published topics
pub = dict()
pub.update({
'talker':
rospy.Publisher('chatter', String, queue_size=10),
'control':
rospy.Publisher('stimulator/ccl_update', Stimulator, queue_size=10),
'angle':
rospy.Publisher('control/angle', Float64, queue_size=10),
'signal':
rospy.Publisher('control/stimsignal', Int32MultiArray, queue_size=10)
})
# define loop rate (in hz)
rate = rospy.Rate(freq)
# build basic angle message
angleMsg = Float64()
# errMsg = Float64()
# build basic stimulator message
stimMsg = Stimulator()
stimMsg.mode = ['single', 'single', 'single', 'single']
stimMsg.pulse_width = [0, 0, 0, 0]
stimMsg.pulse_current = [0, 0, 0, 0]
stimMsg.channel = [1, 2, 3, 4]
control_sel = 0
pw_min_h = 0
pw_min_q = 0
channels_sel = 0
control_onoff = False
ESC_now = [0, 0, 0, 0, 0]
ILC_now = [0, 0, 0]
co_activation = False
initTime = tempo.time()
# load inverse dynamics
try:
ID_data = sio.loadmat(ID_path)
id_pw = ID_data['id_pw'][0]
id_angle = ID_data['id_angle'][0]
# knee_ref = reference_data['knee_ref'][0]
# loaded = 1
except:
print("Ooops, the file you tried to load is not in the folder.")
# knee_ref = [0, 0, 0, 0]
# ---------------------------------------------------- start
while not rospy.is_shutdown():
thisKnee = angle[-1]
thisError = err_angle[-1]
thisRef = refKnee[-1]
thisTime = tempo.time() - initTime
# ============================== >controllers start here
new_kp = Kp_vector[-1]
new_ki = Ki_vector[-1]
new_kd = Kd_vector[-1]
new_kp_hat = Kp_hat_vector[-1]
new_ki_hat = Ki_hat_vector[-1]
new_kd_hat = Kd_hat_vector[-1]
newIntegralError = 0
new_u = 0
jcost = controller.jfunction(thisError, freq) # cost function
if not co_activation:
co_act_h = 0
co_act_q = 0
else:
co_act_h = pw_min_h
co_act_q = pw_min_q
if control_onoff:
# Open-loop inverse dynamics
if control_sel == 0:
try:
res = next(x for x in id_angle if x > thisRef)
# print list(id_angle).index(res)
except StopIteration:
res = 0
print "Not in recruitment curve"
new_u = res / 500
# if thisError < 0:
# new_u = -1
# else:
# new_u = 1
# PID
elif control_sel == 1:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# PID-ILC
elif control_sel == 2:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
# first do PID
thisU_PID, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# then ILC
ilc_send[0] = ilc_memory[0][ilc_i]
ilc_send[1] = ilc_memory[1][ilc_i]
# only change new_u after the second cycle
if ilc_memory[0][ilc_i] == 0:
new_u = thisU_PID
else:
ILC_now[0] = ILC_alpha_now
ILC_now[1] = ILC_beta_now
ILC_now[2] = ILC_gama_now
new_u = controller.pid_ilc(ilc_send, ILC_now, thisU_PID,
ilc_i)
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update ilc_memory
ilc_memory[0][ilc_i] = thisError
ilc_memory[1][ilc_i] = new_u
# update counter
if ilc_i < len(ilc_memory[0]) - 1:
ilc_i = ilc_i + 1
else:
ilc_i = 0
# PID-ES
elif control_sel == 3:
# calculate new pid parameters using ES
ESC_now[0] = ES_A_now
ESC_now[1] = ES_omega_now
ESC_now[2] = ES_phase_now
ESC_now[3] = ES_RC_now
ESC_now[4] = ES_K_now
new_kp, new_kp_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kp_vector[-1], Kp_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kp
ESC_now[0] = ES_A_now / 4 # ES_A_now
# ESC_now[1] = ES_omega_now-2
ESC_now[2] = ES_phase_now + 0.1745
# ESC_now[3] = ES_RC_now/2
# ESC_now[4] = ES_K_now/2
new_ki, new_ki_hat = controller.pid_es2(
jcost, jcost_vector[-1], Ki_vector[-1], Ki_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new ki
ESC_now[0] = ES_A_now / 16 # ES_A_now
# ESC_now[1] = ES_omega_now-3
ESC_now[2] = ES_phase_now + 0.523
# ESC_now[3] = ES_RC_now/8
# ESC_now[4] = ES_K_now/8
new_kd, new_kd_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kd_vector[-1], Kd_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kd
# do PID
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# CR
elif control_sel == 4: # curve recruitment
if control_enable_quad == 1:
new_u = curveRecr[-1]
elif control_enable_hams == 1:
new_u = -curveRecr[-1]
# no controller
else:
print("No controller was selected.")
new_kp = 0
new_ki = 0
new_kd = 0
new_kp_hat = 0
new_ki_hat = 0
new_kd_hat = 0
jcost = 0
newIntegralError = 0
new_u = 0
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update vectors
Kp_vector.append(new_kp)
Ki_vector.append(new_ki)
Kd_vector.append(new_kd)
Kp_hat_vector.append(new_kp_hat)
Ki_hat_vector.append(new_ki_hat)
Kd_hat_vector.append(new_kd_hat)
jcost_vector.append(jcost)
integralError.append(newIntegralError)
u.append(new_u)
# print refKnee[-1]
# ============================== controllers end here
# u to pw
# new_pw = round(abs(u[-1] * 500) / 10) * 10
# define pw for muscles
if u[-1] < 0:
# new_pw = round(abs(u[-1]) * (500-pw_min_h) + pw_min_h) / 10 * 10 # u to pw
new_pw = round(abs(u[-1]) * (500 - pw_min_h) + pw_min_h) # u to pw
pw_h.append(new_pw)
pw_q.append(co_act_q)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (Q), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_q[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
else:
new_pw = round(abs(u[-1]) * (500 - pw_min_q) + pw_min_q) # u to pw
pw_q.append(new_pw)
pw_h.append(co_act_h)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (H), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_h[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
print(controlMsg)
# define current for muscles
current_q.append(new_current_quad)
current_h.append(new_current_hams)
# update topics
if channels_sel == 1: # right leg
stimMsg.pulse_width = [0, 0, pw_q[-1], pw_h[-1]]
stimMsg.pulse_current = [0, 0, current_q[-1], current_h[-1]]
else: # left leg
stimMsg.pulse_width = [pw_q[-1], pw_h[-1], 0, 0]
stimMsg.pulse_current = [current_q[-1], current_h[-1], 0, 0]
pub['control'].publish(stimMsg) # send stim update
angleMsg.data = thisKnee
pub['angle'].publish(angleMsg) # send imu update
# update timestamp
# ts = tempo.time()
t_control.append(thisTime)
# next iteration
rate.sleep()
# ---------------------------------------------------- save everything
# recreate vectors
angle_err_lists = [angle, err_angle, t_angle]
pid_lists = [Kp_vector, Ki_vector, Kd_vector, integralError, t_control]
stim_lists = [current_q, current_h, pw_q, pw_h, t_control]
ref_lists = [refKnee, t_ref, curveRecr, t_curve]
ilc_lists = [ilc_memory, ILC_now, t_control]
esc_lists = [
ESC_now, t_control, Kp_hat_vector, Ki_hat_vector, Kd_hat_vector,
jcost_vector
]
control_lists = [u, t_control]
gui_lists = ([
t_gui, gui_enable_c, gui_control_sel, gui_step_time, gui_kp, gui_ki,
gui_kd, gui_alpha, gui_beta, gui_gama, gui_A, gui_omega, gui_phase,
gui_channels_sel, gui_pw_min_q, gui_pw_min_h, gui_K, gui_RC
])
path_lists = ([gui_ref_param])
steps_lists = ([t_steps, count_steps])
# create name of the file
currentDT = datetime.datetime.now()
# path_str = "/home/bb8/Desktop/gait_"
path_str = gui_save_param[-1]
if channels_sel == 1: # right leg
leg_str = '_R_'
else: # left leg
leg_str = '_L_'
if control_sel == 0:
control_str = '0_'
elif control_sel == 1:
control_str = '1_'
elif control_sel == 2:
control_str = '2_'
elif control_sel == 3:
control_str = '3_'
elif control_sel == 4:
control_str = '4_'
else:
control_str = 'x_'
if not co_activation:
coact_str = "_noCA"
else:
coact_str = "_CA"
currentDT_str = str(currentDT)
name_file = path_str + coact_str + leg_str + control_str + currentDT_str + ".mat"
# save .mat
sio.savemat(
name_file, {
'angle_err_lists': angle_err_lists,
'pid_lists': pid_lists,
'stim_lists': stim_lists,
'ref_lists': ref_lists,
'ilc_lists': ilc_lists,
'control_lists': control_lists,
'esc_lists': esc_lists,
'gui_lists': gui_lists,
'path_lists': path_lists,
'steps_lists': steps_lists
})
def main():
global stimMsg
# init control node
controller = control.Control(rospy.init_node('control', anonymous=False))
# get control config
config_dict = rospy.get_param('/ema_trike/control')
# list subscribed topics
sub = {}
sub['pedal'] = rospy.Subscriber('imu/pedal', Imu, callback=pedal_callback)
sub['remote'] = rospy.Subscriber('imu/remote_buttons',
Int8,
callback=remote_callback)
# list published topics
pub = {}
pub['control'] = rospy.Publisher('stimulator/ccl_update',
Stimulator,
queue_size=10)
pub['angle'] = rospy.Publisher('control/angle', Float64, queue_size=10)
pub['speed'] = rospy.Publisher('control/speed', Float64, queue_size=10)
# define loop rate (in hz)
rate = rospy.Rate(50)
# build basic stimulator message
stimMsg = Stimulator()
stimMsg.channel = [1, 2]
stimMsg.mode = ['single', 'single']
stimMsg.pulse_current = [18, 18]
# build basic angle message
angleMsg = Float64()
# build basic speed message
speedMsg = Float64()
# node loop
while not rospy.is_shutdown():
# calculate control signal
if on_off == True:
pwl, pwr = controller.calculate(angle[-1], speed[-1], speed_ref,
speed_err)
else:
pwl, pwr = [0, 0]
# send stimulator update
stimMsg.pulse_width = [pwl, pwr]
pub['control'].publish(stimMsg)
# send angle update
angleMsg.data = angle[-1]
pub['angle'].publish(angleMsg)
# send speed update
speedMsg.data = speed[-1]
pub['speed'].publish(speedMsg)
# store control signal for plotting
pw_left.append(pwl)
pw_right.append(pwr)
# wait for next control loop
rate.sleep()
# estado atual state = state0 # lista com todos estados, para implementação de botões usando outra imu stateList = [state0, state1, state2, state3, state4, state5] ################################################## ##### CANAIS DO ELETROESTIMIULADOR################ ################################################## ##### DESATIVAR CANAIS NAO UTILIZADOS ############ ##### EM EM STIMULATOR.YALM ##################### ################################################## #testar se desativar canais no stimulator.yalm é suficiente, para não ter necessidade de alterar o código aqui stimMsg = Stimulator() #[quadrícepsDireito, ísquiosDireito, flexorQuadrilDireito, quadrícepsEsquerdo, ísquiosEsquerdo, flexorQuadrilEsquerdo] #stimMsg.channel = [1, 2, 3, 4, 5, 6] #stimMsg.mode = ['single', 'single', 'single', 'single', 'single', 'single', 'single', 'single'] ##stimMsg.pulse_current = [2, 2, 2, 2, 2, 2, 2, 2] # currenet in mA ##stimMsg.pulse_width = [0, 0, 0, 0, 0, 0, 0, 0] #[quadrícepsDireito, ísquiosDireito, quadrícepsEsquerdo, ísquiosEsquerdo] stimMsg.channel = [1, 2, 3, 4, 5, 6] stimMsg.mode = ['single'] * 6 stimMsg.pulse_current = [2, 2, 2, 2, 2, 2] # calibrar pra cada paciente stimMsg.pulse_width = [0] * 6 ################################################## ##### Loop do ROS ################################