self.pub_range_left = rospy.Publisher("range_left", Range, queue_size=16)
self.pub_range_right = rospy.Publisher("range_right", Range, queue_size=16)
self.cmd_vel = None
+ self.cur_vel = (0, 0)
self.bMotorManual = False
self.set_speed(0, 0)
rospy.loginfo("Init done")
i+=1
if self.cmd_vel != None:
self.set_speed(self.cmd_vel[0], self.cmd_vel[1])
+ self.cur_vel = self.cmd_vel
self.cmd_vel = None
rate.sleep()
return config
def imuReceived(self, msg):
- if self.rollover_protect:
+ if self.rollover_protect and any(self.cur_vel):
(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
+ self.cmd_vel = (0, 0)
+ rospy.logwarn("Rollver protection done")
def get_reset(self):
reset = struct.unpack(">B", i2c_read_reg(0x50, 0xA0, 1))[0]