def get_pos_in(self):
return m3u.m2in(self.get_pos_m())
def get_pos_in(self):
return m3u.m2in(self.get_pos_m())
def calibrate_torque_ext_spring(self):
#temporary, no 12bit encoder available for j0 calibration
if self.jid == 0:
self.theta.ctype = 'ma3_10bit'
print 'This routine requires actuator_ec PID control and encoder feedback of the extension spring'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
if self.jid == 0:
print 'Routine not valid for joint 0'
return
print 'Enter amplitude [', self.param_internal['tq_ext_amp'], ']'
amp = m3t.get_float(self.param_internal['tq_ext_amp'])
print 'Moving capstan to unloaded position. Enable power. Hit enter when ready'
raw_input()
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire = int(
self.param_internal['tq_ext_windup'])
self.step(4.0)
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF
self.step()
print 'Unload gearhead by pushing on spring arm'
print 'Place actuator in unloaded configuration. Hit enter when ready'
raw_input()
self.step()
adc_zero = float(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
enc_zero = self.theta.raw_2_deg(self.comp_rt.config['calib']['theta'],
self.comp_ec.status.qei_on,
self.comp_ec.status.qei_period,
self.comp_ec.status.qei_rollover)
ns = 500.0
t = nu.arange(.75, -.25 - 1.0 / ns, -1.0 / ns)
print 'T', t
des = adc_zero + amp * (.5 * nu.sin(t * 2 * math.pi) + .5)
print 'DES', des
log_adc_torque = []
log_enc = []
log_mNm = []
#Init
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire = int(adc_zero)
print 'Connect extension spring. Enable motor power. Hit enter when ready'
raw_input()
self.step(time_sleep=1.0)
for idx in range(len(des)):
self.comp_ec.command.t_desire = int(des[idx])
self.step(time_sleep=0.05)
enc = self.theta.raw_2_deg(self.comp_rt.config['calib']['theta'],
self.comp_ec.status.qei_on,
self.comp_ec.status.qei_period,
self.comp_ec.status.qei_rollover)
print 'Des', des[idx], 'Enc', enc - enc_zero
if abs(enc - enc_zero) > self.param_internal['tq_ext_deadband']:
ks = self.param_internal['tq_ext_ks']
r = self.param_internal['r_capstan'] / 1000.0 #m
dx = 2 * math.pi * m3u.m2in(r) * (enc -
enc_zero) / 360.0 #inches
lb = dx * ks
mN = m3u.Lb2mN(lb)
tq_enc = mN * r
tq_raw = self.comp_ec.status.adc_torque
print '------------------------'
print 'Des', idx, '/', len(t), ': ', t[idx]
print 'Adc SEA', tq_raw
print 'Lb spring', lb
print 'tq spring', tq_enc
log_adc_torque.append(tq_raw)
log_mNm.append(tq_enc)
log_enc.append(enc - enc_zero)
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF
self.step()
print 'Poly degree [', self.param_internal['calib_tq_degree'], ']?'
n = m3t.get_int(self.param_internal['calib_tq_degree'])
poly, inv_poly = self.get_polyfit_to_data(x=log_adc_torque,
y=log_mNm,
n=n)
self.write_raw_calibration({
'log_adc_torque': log_adc_torque,
'log_mNm': log_mNm,
'log_enc': log_enc,
'cb_torque': poly,
'cb_inv_torque': inv_poly
})
self.comp_rt.config['calib']['torque']['cb_torque'] = poly
self.comp_rt.config['calib']['torque']['cb_inv_torque'] = inv_poly
print 'Poly', poly
s = m3tc.PolyEval(poly, log_adc_torque)
m3t.mplot2(range(len(log_adc_torque)),
log_mNm,
s,
xlabel='Samples',
ylabel='Torque (mNm)',
y1name='loadcell',
y2name='actuator')
def calibrate_torque_ext_spring(self):
#temporary, no 12bit encoder available for j0 calibration
if self.jid==0:
self.theta.ctype='ma3_10bit'
print 'This routine requires actuator_ec PID control and encoder feedback of the extension spring'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
if self.jid==0:
print 'Routine not valid for joint 0'
return
print 'Enter amplitude [',self.param_internal['tq_ext_amp'],']'
amp=m3t.get_float(self.param_internal['tq_ext_amp'])
#print 'Moving capstan to unloaded position. Enable power. Hit enter when ready'
#raw_input()
#self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
#self.comp_ec.command.t_desire=int(self.param_internal['tq_ext_windup'])
#self.step(4.0)
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF
self.step()
self.step()
adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
enc_zero=self.theta.raw_2_deg(self.comp_rt.config['calib']['theta'],
self.comp_ec.status.qei_on,self.comp_ec.status.qei_period,self.comp_ec.status.qei_rollover)
ns=500.0
t=nu.arange(.75,-.25-1.0/ns,-1.0/ns)
print 'T',t
des=adc_zero+amp*(.5*nu.sin(t*2*math.pi)+.5)
print 'DES',des
log_adc_torque=[]
log_enc=[]
log_mNm=[]
#Init
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire=int(adc_zero)
print ''
print 'Connect extension spring. Enable motor power. Hit enter when ready'
raw_input()
self.step(time_sleep=1.0)
for idx in range(len(des)):
self.comp_ec.command.t_desire=int(des[idx])
self.step(time_sleep=0.05)
enc=self.theta.raw_2_deg(self.comp_rt.config['calib']['theta'],
self.comp_ec.status.qei_on,self.comp_ec.status.qei_period,self.comp_ec.status.qei_rollover)
print 'Des',des[idx],'Enc',enc-enc_zero
if abs(enc-enc_zero)>self.param_internal['tq_ext_deadband']:
ks=self.param_internal['tq_ext_ks']
r=self.param_internal['r_capstan']/1000.0 #m
dx=2*math.pi*m3u.m2in(r)*(enc-enc_zero)/360.0 #inches
lb=dx*ks
mN=m3u.Lb2mN(lb)
tq_enc=mN*r
tq_raw=self.comp_ec.status.adc_torque
print '------------------------'
print 'Des',idx,'/',len(t),': ',t[idx]
print 'Adc SEA',tq_raw
print 'Lb spring',lb
print 'tq spring',tq_enc
log_adc_torque.append(tq_raw)
log_mNm.append(tq_enc)
log_enc.append(enc-enc_zero)
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF
self.step()
print ''
print 'Disable Power'
#m3t.mplot(range(len(log_adc_torque)),log_adc_torque)
print 'Poly degree [',self.param_internal['calib_tq_degree'],']?'
n=m3t.get_int(self.param_internal['calib_tq_degree'])
poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_mNm,n=n)
self.write_raw_calibration({'log_adc_torque':log_adc_torque,'log_mNm':log_mNm,'log_enc':log_enc,
'cb_torque':poly,'cb_inv_torque':inv_poly})
self.comp_rt.config['calib']['torque']['cb_torque']=poly
self.comp_rt.config['calib']['torque']['cb_inv_torque']=inv_poly
print 'Poly',poly
s=m3tc.PolyEval(poly,log_adc_torque)
m3t.mplot2(range(len(log_adc_torque)),log_mNm,s,xlabel='Samples',ylabel='Torque (mNm)',
y1name='loadcell',y2name='actuator')
def calibrate_torque_ext_spring(self):
# temporary, no 12bit encoder available for j0 calibration
if self.jid == 0:
self.theta.ctype = "ma3_10bit"
print "This routine requires actuator_ec PID control and encoder feedback of the extension spring"
print "Proceed [y]?"
if not m3t.get_yes_no("y"):
return
if self.jid == 0:
print "Routine not valid for joint 0"
return
print "Enter amplitude [", self.param_internal["tq_ext_amp"], "]"
amp = m3t.get_float(self.param_internal["tq_ext_amp"])
print "Moving capstan to unloaded position. Enable power. Hit enter when ready"
raw_input()
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire = int(self.param_internal["tq_ext_windup"])
self.step(4.0)
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF
self.step()
print "Unload gearhead by pushing on spring arm"
print "Place actuator in unloaded configuration. Hit enter when ready"
raw_input()
self.step()
adc_zero = float(self.get_sensor_list_avg(["adc_torque"], 1.0)["adc_torque"])
enc_zero = self.theta.raw_2_deg(
self.comp_rt.config["calib"]["theta"],
self.comp_ec.status.qei_on,
self.comp_ec.status.qei_period,
self.comp_ec.status.qei_rollover,
)
ns = 500.0
t = nu.arange(0.75, -0.25 - 1.0 / ns, -1.0 / ns)
print "T", t
des = adc_zero + amp * (0.5 * nu.sin(t * 2 * math.pi) + 0.5)
print "DES", des
log_adc_torque = []
log_enc = []
log_mNm = []
# Init
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire = int(adc_zero)
print "Connect extension spring. Enable motor power. Hit enter when ready"
raw_input()
self.step(time_sleep=1.0)
for idx in range(len(des)):
self.comp_ec.command.t_desire = int(des[idx])
self.step(time_sleep=0.05)
enc = self.theta.raw_2_deg(
self.comp_rt.config["calib"]["theta"],
self.comp_ec.status.qei_on,
self.comp_ec.status.qei_period,
self.comp_ec.status.qei_rollover,
)
print "Des", des[idx], "Enc", enc - enc_zero
if abs(enc - enc_zero) > self.param_internal["tq_ext_deadband"]:
ks = self.param_internal["tq_ext_ks"]
r = self.param_internal["r_capstan"] / 1000.0 # m
dx = 2 * math.pi * m3u.m2in(r) * (enc - enc_zero) / 360.0 # inches
lb = dx * ks
mN = m3u.Lb2mN(lb)
tq_enc = mN * r
tq_raw = self.comp_ec.status.adc_torque
print "------------------------"
print "Des", idx, "/", len(t), ": ", t[idx]
print "Adc SEA", tq_raw
print "Lb spring", lb
print "tq spring", tq_enc
log_adc_torque.append(tq_raw)
log_mNm.append(tq_enc)
log_enc.append(enc - enc_zero)
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF
self.step()
print "Poly degree [", self.param_internal["calib_tq_degree"], "]?"
n = m3t.get_int(self.param_internal["calib_tq_degree"])
poly, inv_poly = self.get_polyfit_to_data(x=log_adc_torque, y=log_mNm, n=n)
self.write_raw_calibration(
{
"log_adc_torque": log_adc_torque,
"log_mNm": log_mNm,
"log_enc": log_enc,
"cb_torque": poly,
"cb_inv_torque": inv_poly,
}
)
self.comp_rt.config["calib"]["torque"]["cb_torque"] = poly
self.comp_rt.config["calib"]["torque"]["cb_inv_torque"] = inv_poly
print "Poly", poly
s = m3tc.PolyEval(poly, log_adc_torque)
m3t.mplot2(
range(len(log_adc_torque)),
log_mNm,
s,
xlabel="Samples",
ylabel="Torque (mNm)",
y1name="loadcell",
y2name="actuator",
)