sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag
reset_val = self.get_reset()
rospy.loginfo("Reset Status: 0x%x" % reset_val)
- ir_count = 0
sonar_count = 0
i2c_write_reg(0x50, 0xA4, struct.pack("B", 1)) # enable Watchdog
while not rospy.is_shutdown():
self.get_odom()
self.get_power()
- if ir_count == 0:
- self.get_dist_left()
- ir_count+=1
- else:
- self.get_dist_right()
- ir_count=0
-
if sonar_count == 0:
self.get_dist_forward_left()
self.update_dist_backward()