def __init__(self):
"""
コンストラクタ
"""
self.cyclecount = 0
# 受信作成
self.sub_brain = rospy.Subscriber('client2',
brain,
self.clientCallback,
queue_size=1)
# 送信作成
self.pub_arm = rospy.Publisher('arm', arm, queue_size=100)
# messageのインスタンスを作る
self.msg_arm = arm()
# 送信メッセージ初期化
self.clearMsg()
# メッセージ受信用変数
#self.drill_req = 0 # 受信値B
# MortorClass
self.stmc = mortor.StepMortorClass(DEBUG_ARM,
(PORT_HANDV_A, PORT_HANDV_B),
(LIM_HANDV_MIN, LIM_HANDV_MAX))
self.svmc = mortor.ServoMortorClass(DEBUG_ARM)
def __init__(self):
self.is_enable = False
try:
self.smc = mortor.ServoMortorClass(False)
self.i2c = smbus.SMBus(1)
# ICの設定
setdata = (SET_RST << 12) + (SET_AVG << 9) + \
(SET_VBUSCT << 6) + (SET_VSHCT << 3) + SET_MODE
setdata = ((setdata << 8) & 0xFF00) + (setdata >> 8)
self.i2c.write_word_data(I2C_INA226, ADDR_S, setdata)
# キャリブレーションレジスタの設定
# 0.00512/((0.0015[mΩ])*0.001)
setdata = int(0.00512/((BATT_R)*0.001))
setdata = ((setdata << 8) & 0xFF00) + (setdata >> 8)
self.i2c.write_word_data(I2C_INA226, ADDR_R, setdata)
self.is_enable = True
# 1回目は変な値をとるときが多いので...
self.read_v()
self.read_i()
except IOError:
self.is_enable = False
finally:
self.is_battlow = False
self.cnt_battlow = 0
self.v_ave = 0
self.i_ave = 0
self.i_sgm = 0
def main_servo():
"""
サーボモーターテスト
"""
print("15号(%d) 刈りん(%d)" % (TARGET.TOMATO, TARGET.GRASS))
tgt = input('target:')
int_tgt = TARGET(tgt)
if (int_tgt == TARGET.SIDE_SPROUT) or (int_tgt == TARGET.TOMATO) or (int_tgt == TARGET.GRASS):
smc = mortor.ServoMortorClass()
else:
return
if (int_tgt == TARGET.SIDE_SPROUT) or (int_tgt == TARGET.TOMATO):
for channel in enumerate(CHANNEL_M):
pass
# smc.posinit(channel)
elif int_tgt == TARGET.GRASS:
for channel in enumerate(CHANNEL_K):
pass
# smc.posinit(channel)
else:
pass
while True:
try:
if (int_tgt == TARGET.SIDE_SPROUT) or (int_tgt == TARGET.TOMATO):
print("[15号]\n(0)ハンド\t(1)手首 \n(2)枝掴み\t(3)枝ねじり\n(4)添え手右\t(5)添え手左")
elif int_tgt == TARGET.GRASS:
print(
"[刈りん]\n(0)ハンド\t(1)手首 \n(2)引抜 \t(3)肘 \n(4)肩 \t(5)土台\n(6)蓋")
else:
break
mortornum = input('mortor:')
int_port = int(mortornum)
if int_port < 0:
break
val = input(' pulse:')
int_tmp = int(val)
if int_tmp < 0:
break
smc.move_servo_pulse(int_port, int_tmp)
except KeyboardInterrupt:
print("Ctrl+Cで停止しました")
break
except TypeError as ex:
print(ex)
smc.endfnc()
def __init__(self):
"""
コンストラクタ
"""
# 受信作成
self.sub_armdebug = rospy.Subscriber('armdebug',
armdebug,
self.armdebugCallback,
queue_size=1)
# 送信作成
self.pub_servo = rospy.Publisher('servo', servo, queue_size=100)
# messageのインスタンスを作る
self.msg_servo = servo()
# 送信メッセージ初期化
self.clearMsg()
# MortorClass
self.svmc = mortor.ServoMortorClass(DEBUG_ARM)
def __init__(self, noros=False):
# index
self.frame_id = 0
self.noros = noros
# パブリッシャーの準備
if not (self.noros):
self.pub_arm = rospy.Publisher('arm', arm, queue_size=100)
self.pub_body = rospy.Publisher('body', body, queue_size=100)
self.pub_hand = rospy.Publisher('hand', hand, queue_size=100)
#end if not(noros)
self.mes_arm = arm()
self.mes_body = body()
self.mes_hand = hand()
self.mes_brain = brain()
self.is_arm_move = False
self.is_body_move = False
self.is_hand_move = False
self.is_abh_call = False
# arm用
self.elbow_req_o = 0 # 肘モーター要求値
self.should_req_o = 0 # 肩モーター要求値
self.handx_req_o = 0 # ハンド指定位置X
self.handy_req_o = 0 # ハンド指定位置Y
self.handz_req_o = 0 # ハンド指定位置Z
self.twistx_req_o = 0 # ねじ切りハンド指定位置X
self.twistz_req_o = 0 # ねじ切りハンド指定位置Z
# body用
self.is_pwoffsw = False
# hand用
# MortorClass
self.dmc = mortor.DcMortorClass(DEBUG_BODY,
(PORT_BLADE_A, PORT_BLADE_B))
self.stmc_handh = mortor.StepMortorClass(
DEBUG_ARM, (PORT_HANDH_A, PORT_HANDH_B),
(LIM_HANDH_MIN, LIM_HANDH_MAX))
self.stmc_handv = mortor.StepMortorClass(
DEBUG_ARM, (PORT_HANDV_A, PORT_HANDV_B),
(LIM_HANDV_MIN, LIM_HANDV_MAX))
self.stmc_twisth = mortor.StepMortorClass(
DEBUG_ARM, (PORT_TWISTH_A, PORT_TWISTH_B),
(LIM_TWISTH_MIN, LIM_TWISTH_MAX))
self.stmc_twistv = mortor.StepMortorClass(
DEBUG_ARM, (PORT_TWISTV_A, PORT_TWISTV_B),
(LIM_TWISTV_MIN, LIM_TWISTV_MAX))
self.svmc = mortor.ServoMortorClass((DEBUG_ARM or DEBUG_HAND),
TARGET.UNKNOWN)
# ina226Class
self.inac = ina226.Ina226Class()
# initialize port
self.pic = pigpio.pi()
self.pic.set_mode(PORT_PWOFFSW, pigpio.INPUT)
# モード今回値
self.mode_now = MODE.UNKNOWN
self.target_now = TARGET.UNKNOWN
self.elbow_req_o = 0 # 肘モーター要求値前回値
self.should_req_o = 0 # 肩モーター要求値前回値
self.handx_req_o = 0 # ハンド指定位置X前回値
self.handy_req_o = 0 # ハンド指定位置Y前回値
self.handz_req_o = 0 # ハンド指定位置Z前回値
self.twistx_req_o = 0 # ねじ切りハンド指定位置X前回値
self.twistz_req_o = 0 # ねじ切りハンド指定位置Z前回値
self.elbow_o = 0 # 肘モーター
self.should_o = 0 # 肩モーター
self.base_o = 0 # 土台モーター
self.twistv_o = 0 # ねじ切り垂直モーター
self.twisth_o = 0 # ねじ切り水平モーター
self.handv_o = 0 # ハンド垂直モーター
self.handh_o = 0 # ハンド水平モーター
self.elbow_req_o = 0 # 肘モーター要求値前回値
# debug用
self.arm_grass_hand = 0
self.arm_grass_pluck = 0
self.arm_grass_wrist = 0
self.arm_grass_elbow = 0
self.arm_grass_sholder = 0
self.arm_grass_base = 0
self.arm_grass_lid = 0
self.arm_grass_splay = 0
self.arm_grass_blade = 0
self.arm_crop_hand = 0
self.arm_crop_wrist = 0
self.arm_crop_vertical = 0
self.arm_crop_horizontal = 0
self.arm_sprout_grub = 0
self.arm_sprout_twist = 0
self.arm_sprout_attach_r = 0
self.arm_sprout_attatch_l = 0
self.arm_sprout_vertical = 0
self.arm_sprout_horizontal = 0