def main_step():
"""
ステッピングモーターテスト
"""
smc_handh = mortor.StepMortorClass(
False, (PORT_HANDH_A, PORT_HANDH_B), (-LIM_HANDH_MAX, LIM_HANDH_MAX))
smc_handv = mortor.StepMortorClass(
False, (PORT_HANDV_A, PORT_HANDV_B), (-LIM_HANDV_MAX, LIM_HANDV_MAX))
smc_twisth = mortor.StepMortorClass(
False, (PORT_TWISTH_A, PORT_TWISTH_B), (-LIM_TWISTH_MAX, LIM_TWISTH_MAX))
smc_twistv = mortor.StepMortorClass(
False, (PORT_TWISTV_A, PORT_TWISTV_B), (-LIM_TWISTV_MAX, LIM_TWISTV_MAX))
while True:
try:
print("(1)ハンド水平\n(2)ハンド垂直\n(3)ねじ切り水平\n(4)ねじ切り垂直")
mortornum = input('mortor:')
if mortornum < 1:
break
val = input(' step:')
int_tmp = int(val)
if int_tmp == 0:
break
if mortornum == 1:
smc_handh.move_step_step(int_tmp)
elif mortornum == 2:
smc_handv.move_step_step(int_tmp)
elif mortornum == 3:
smc_twisth.move_step_step(int_tmp)
elif mortornum == 4:
smc_twistv.move_step_step(int_tmp)
elif mortornum == 5:
smc_handh.posinit(0)
smc_handv.posinit(0)
smc_twisth.posinit(0)
smc_twistv.posinit(0)
else:
break
except KeyboardInterrupt:
print("Ctrl+Cで停止しました")
break
except TypeError as ex:
print(ex)
smc_handh.endfnc()
smc_handv.endfnc()
smc_twisth.endfnc()
smc_twistv.endfnc()
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.sub_armdebug = rospy.Subscriber('armdebug',
armdebug,
self.armdebugCallback,
queue_size=1)
# 送信作成
self.pub_step = rospy.Publisher('step', step, queue_size=100)
# messageのインスタンスを作る
self.msg_step = step()
# 送信メッセージ初期化
self.clearMsg()
# MortorClass
self.stmc = mortor.StepMortorClass(DEBUG_ARM,
(PORT_HANDV_A, PORT_HANDV_B),
(LIM_HANDV_MIN, LIM_HANDV_MAX))
self.pi = pigpio.pi()
self.pi.set_mode(RET_ORGSW, pigpio.INPUT)
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