Example #1
0
    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)
Example #2
0
    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
Example #3
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()
Example #4
0
    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)
Example #5
0
    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