def CasPIDClb():
rootLogger.info("Arriving in CasPIDClb State. ")
PID = [0.0204, 0.13, 0.0037]
CasCtr = ctrlib.PidController_WindUp(PID, TSAMPLING, max_output=.4)
while llc_ref.state == 'CASPIDCLB':
if IMU:
read_imu()
calc_angle(self)
#referenz über pattern
# pattern_ref(patternname='pattern_0.csv', alpha=True)
#referenz über Poti
set_alpha_ref_via_poti()
for name in CHANNELset:
aref = llc_ref.alpha[name]
clb = calibration.get_pressure(aref, version='big')
pid = CasCtr.output(aref, llc_rec.aIMU[name])
pref = clb + pid
pwm = pwm = calibration.cut_off(pref * 100, 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def get_reference(self, alpha):
self.steady_ref = calibration.get_pressure(alpha, self.version)
if self.steady_ref < self.last_steady_ref: # smaller
self.ref = self.steady_ref
elif self.steady_ref > self.last_steady_ref: # greater
self.last_boost = time.time()
self.ref = self.boost_pressure(self.steady_ref)
self.is_active = True
elif (time.time() - self.last_boost > self.tboost) and self.is_active:
self.ref = self.steady_ref
self.is_active = False
self.last_steady_ref = self.steady_ref
return self.ref
def KB_MotionGen():
fun = ui_state.fun
if (fun[0] and kbmg_mgmt.last_process_time + kbmg_mgmt.process_time <
time.time()):
act_pos = (imgproc_rec.X[1], imgproc_rec.Y[1])
act_eps = imgproc_rec.eps if imgproc_rec.eps else np.nan
# xref = imgproc_rec.xref if imgproc_rec.xref[0] else (np.nan, np.nan)
# XREF = [(0, 0), (20, 30), (-45, 50), (20, 95)] # cm
XREF = [
(467., 515), # origin
(783., 288.),
(962., 850.),
(1364., 288.)
]
try:
xref = XREF[kbmg_mgmt.idx]
imgproc_rec.xref = xref # write to recorder
except IndexError:
xref = XREF[-1]
if not np.isnan(xref[0]) and not np.isnan(act_eps):
# convert measurements
xbar = gait_law_planner.xbar(xref, act_pos, act_eps)
xbar = xbar * 112. / 1000 # px to cm
dist = np.linalg.norm(xbar)
deps = np.rad2deg(np.arctan2(xbar[1], xbar[0]))
print('\n\nxbar:\t', [round(x, 2) for x in xbar])
print('dist:\t', round(dist, 2))
print('deps:\t', round(deps, 2))
if dist < 10:
print('\n\nReached goal ', kbmg_mgmt.idx, ' !!\n\n\n')
kbmg_mgmt.idx += 1 if kbmg_mgmt.idx < len(XREF) else 0
else:
# generate reference
alpha, foot, ptime, pose_id = \
kbmg_mgmt.ref_generator.get_next_reference(
act_pos, act_eps, xref)
print(pose_id)
pvtsk, dvtsk = convert_ref(
clb.get_pressure(alpha, mgmt.version), foot)
# send to main thread
llc_ref.dvalve = dvtsk
llc_ref.pressure = pvtsk
# organisation
kbmg_mgmt.process_time = ptime
kbmg_mgmt.last_process_time = time.time()
def PPID():
rootLogger.info("Arriving in PPID State. ")
while llc_ref.state == 'PPID':
if IMU and is_poti():
read_imu()
calc_angle(self)
#referenz über pattern
pattern_ref(patternname='pattern_0.csv', alpha=True)
for name in CHANNELset:
aref = llc_ref.alpha[name]
pref = calibration.get_pressure(aref, version='big')
pwm = calibration.cut_off(int(100 * pref), 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
def optimal_pathplanner():
fun = ui_state.fun
if gl_mgmt.round == 0:
# feasible start pose:
pvtsk, dvtsk = convert_ref(
clb.get_pressure([30, 0, gl_mgmt.lastq1, 30, 0], mgmt.version),
[1, 0, 0, 1])
llc_ref.dvalve = dvtsk
llc_ref.pressure = pvtsk
nmax = 4
max_step_length = 90
if (fun[0] and
gl_mgmt.last_process_time + gl_mgmt.process_time < time.time()):
# collect measurements
position = (imgproc_rec.X[1], imgproc_rec.Y[1])
eps = imgproc_rec.eps if imgproc_rec.eps else np.nan
xref = imgproc_rec.xref if imgproc_rec.xref[0] else (np.nan, np.nan)
if not np.isnan(xref[0]) and not np.isnan(eps):
# convert measurements
xbar = gait_law_planner.xbar(xref, position, eps)
xbar = xbar * 112. / 1000 # px to cm
deps = np.rad2deg(np.arctan2(xbar[1], xbar[0]))
dist = np.linalg.norm(xbar)
print('\n\nxbar:\t', [round(x, 2) for x in xbar])
print('dist:\t', round(dist, 2))
print('deps:\t', round(deps, 2))
pressure_ref, feet_act = convert_rec(llc_ref.pressure,
llc_ref.dvalve)
alp_act = clb.get_alpha(pressure_ref, mgmt.version)
# calc ref
[alpha,
feet], q = gait_law_planner.optimal_planner(xbar,
alp_act,
feet_act,
gl_mgmt.lastq1,
nmax=nmax,
q1bnds=[50, 90])
gl_mgmt.lastq1 = q[0]
pattern = [[alp_act, [1, 1, 1, 1], .2], [alp_act, feet, .1],
[alpha, feet, 1.5]]
for idx, pose in enumerate(pattern):
alpha, feet, ptime = pose
# convert ref
pvtsk, dvtsk = convert_ref(
clb.get_pressure(alpha, mgmt.version), feet)
# set ref
llc_ref.dvalve = dvtsk
llc_ref.pressure = pvtsk
if idx != len(pattern) - 1: # last pose -> no sleep
time.sleep(ptime)
print('q:\t\t', [round(qi, 2) for qi in q])
print('alpha:\t', [int(a) for a in alpha])
print('pres:\t', [
int(p * 100) / 100.
for p in clb.get_pressure(alpha, mgmt.version)
])
# print('feet:\t', feet)
print('\n\n---------------------------------\n\n')
# organisation
gl_mgmt.process_time = ptime
gl_mgmt.last_process_time = time.time()
gl_mgmt.round += 1
else:
print('No detection ...')