imuTurtle.py 文件源码

def strength(emgArr1):
        emgArr=emgArr1.data
        # Define proportional control constant:
        K = 0.005
        # Get the average muscle activation of the left, right, and all sides
        aveRight=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3])/4
        aveLeft=(emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/4
        ave=(aveLeft+aveRight)/2
        # If all muscles activated, drive forward exponentially
        if ave > 500:
            tsPub.publish(Twist(Vector3(0.1*math.exp(K*ave),0,0),Vector3(0,0,0)))
        # If only left muscles activated, rotate proportionally
        elif aveLeft > (aveRight + 200):
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,K*ave)))
        # If only right muscles activated, rotate proportionally
        elif aveRight > (aveLeft + 200):
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,-K*ave)))
        # If not very activated, don't move (high-pass filter)
        else:
            tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0)))