From 6f9283b6b41b8e5fcfd346170ba231c35e8b938c Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Tue, 11 Apr 2017 07:56:46 +0200 Subject: [PATCH] rollover protection: Also stop unused motors --- scripts/wt_node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 3d3adef..afef56a 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -115,15 +115,16 @@ class MoveBase: (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__()) if pitch > self.rollover_protect_limit*pi/180: self.bMotorManual = True - i2c_write_reg(0x50, 0x3, struct.pack(">h", self.rollover_protect_pwm)) - i2c_write_reg(0x50, 0x7, struct.pack(">h", self.rollover_protect_pwm)) + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", 0, self.rollover_protect_pwm, 0, self.rollover_protect_pwm)) + rospy.logwarn("Running forward rollver protection") elif pitch < -self.rollover_protect_limit*pi/180: self.bMotorManual = True - i2c_write_reg(0x50, 0x1, struct.pack(">h", -self.rollover_protect_pwm)) - i2c_write_reg(0x50, 0x5, struct.pack(">h", -self.rollover_protect_pwm)) + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", -self.rollover_protect_pwm, 0, -self.rollover_protect_pwm, 0)) + rospy.logwarn("Running backward rollver protection") elif self.bMotorManual: i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", 0, 0, 0, 0)) self.bMotorManual = False + rospy.logwarn("Rollver protection done") def get_reset(self): reset = struct.unpack(">B", i2c_read_reg(0x50, 0xA0, 1))[0] -- 2.39.5