Question

我目前正在研究一个必须穿越迷宫的机器人。对于机器人，我使用的是TMC222步进控制器，软件用Python编码。

我需要的功能可以告诉我电机何时繁忙，以便机器人在电机运转时抓住所有其他活动。

我的想法是检查电机上的当前位置并将其与目标位置进行比较，但我还没有将它运行起来。

我目前的尝试：

        def isRunning(self):
        print("IS RUNNING TEST")

        fullstatus=self.getFullStatus2()

        #print("FULL STATUS: " + str(fullstatus[0]) + " 2 " + str(fullstatus[1]))

        actLeft=fullstatus[0][1]<<8 | fullstatus[0][2]<<0
        actRight=fullstatus[1][1]<<8 | fullstatus[1][2]<<0

        tarLeft=fullstatus[0][3]<<8 | fullstatus[0][4]<<0
        tarRight=fullstatus[1][3]<<8 | fullstatus[1][4]<<0

        value = (actLeft==tarLeft) and (actRight==tarRight)

        value = not value
#       print("isbusy="+str(value))
        print 'ActPos = ' + str(actLeft)
        print 'TarPos = ' + str(tarLeft)

        return value

Answer 1

查看您的getFullStatus2()代码会很有帮助，因为我不清楚您是如何获得多维输出的。

通常，您可以像两个8位字节一样形成一个16位“字”：

Word = HB << 8 | LB << 0

其中HB和LB是高（位15-8）和低（位7-0）字节。

话虽如此，有多种方法可以检测电机失速。理想的方式是外部压力开关，当它撞到墙壁时关闭。另一种方法是监控电机的电流，当电机面临电阻时（加速或失速时），电流会上升。

由于看起来这些都不可能，我会使用不同的方法，随着时间的推移监控电机的位置（可能来自某种编码器）。

假设您有一个函数get_position()，它返回一个无符号的16位整数。你应该能够写出类似的东西：

class MotorPosition(object):
    def __init__(self):
        readings = []

    def poll(self):
        p = get_position()
        self.readings.append(readings)

        # If the list is now too long, remove the oldest entries
        if len(self.readings) > 5:
            self.readings.pop(0)

    def get_deltas():
        deltas = []
        for x,y in zip(self.readings[1:4], self.readings[0:3]):
            d = x - y
            # Wraparound detection
            if  (d < -THRESHOLD): d += 65536
            elif(d >  THRESHOLD): d -= 65536
            deltas.append(d)
       return deltas

    def get_average_delta():
        deltas = self.get_deltas()
        return sum(deltas) / float(len(deltas))

请注意，这假设您以足够快的速度和频率一致地轮询编码器。

然后您可以监控平均增量（来自get_average_delta()），如果它低于某个值，您会认为电机停转。

假设：

This是您正在使用的控制器的数据表
您的I²C代码工作正常

TMC222步进控制器，电机忙功能

1 个答案: