X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fwt_node.py;h=5056107206766930273850efc4dc6a8ce579a222;hp=4064fb89a364ec91e9aaa7c9c5da2c8fe5607ade;hb=2ee98f31c1f2527cdb4e190e37d9f6ad9e246859;hpb=3c4bb7f988be07a6dd592c80541e73135536a96c diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 4064fb8..5056107 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -65,10 +65,10 @@ class MoveBase: (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__()) if pitch > 30*pi/180: val = (100.0/65)*abs(pitch)*180/pi - self.set_motor_handicap(0, int(val)) + self.set_motor_handicap(int(val), 0) elif pitch < -30*pi/180: val = (100.0/65)*abs(pitch)*180/pi - self.set_motor_handicap(int(val), 0) + self.set_motor_handicap(0, int(val)) else: self.set_motor_handicap(0, 0)