analogTurtle.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)))