def twist_callback(msg):
global pub
global last_time
last_time = rospy.get_rostime()
#rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
#rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
TR = MIX_ROTATION
v_l = msg.linear.x - TR * msg.angular.z
v_r = msg.linear.x + TR * msg.angular.z
dutyleft = int(v_l * MAX_DUTY_CYCLE * 32768)
dutyright = int(v_r * MAX_DUTY_CYCLE * 32768)
dutyleft = limit(dutyleft, 32767)
dutyright = limit(dutyright, 32767)
rc.DutyM1M2(MTR_ADDRESS, dutyleft, dutyright)
rospy.loginfo("[%d , %d]" % (dutyleft, dutyright))
dummy, c1, c2 = rc.ReadCurrents(MTR_ADDRESS)
status = rc.ReadError(MTR_ADDRESS)[1]
bvoltage = rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0
diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (
bvoltage, c1 / 100.0, c2 / 100.0, status)
pub.publish(diagstr)
def debug_robot():
rc.Open('/dev/ttyUSB0',115200)
while True:
status = rc.ReadError(MTR_ADDRESS)[1]
dummy,c1,c2 = rc.ReadCurrents(MTR_ADDRESS)
bvoltage =rc.ReadMainBatteryVoltage(MTR_ADDRESS)[1] / 10.0
diagstr = "BattVoltage %f, Current[%f,%f], Status 0x%x" % (bvoltage, c1/100.0, c2/100.0, status)
print diagstr
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
with self.lock:
if (rospy.get_rostime() -
self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get comand for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
# TODO need find solution to the OSError11 looks like sync problem with serial
status1, enc1, crc1 = None, None, None
status2, enc2, crc2 = None, None, None
statusC, amp1, amp2 = None, None, None
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
try:
status1c, amp1, amp2 = roboclaw.ReadCurrents(self.address)
except ValueError:
pass
if (enc1 != None) & (enc2 != None):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
else:
rospy.logdebug("Error Reading enc")
if (amp1 != None) & (amp2 != None):
rospy.logdebug(" Currents %d %d" % (amp1, amp2))
amps = Motors_currents()
amps.motor1 = float(amp1) / 100
amps.motor2 = float(amp2) / 100
self.motors_currents_pub.publish(amps)
else:
rospy.logdebug("Error Reading Currents")
r_time.sleep()
def displayCurrent():
crc, l_currentA, r_currentA = rc.ReadCurrents(address)
elapsed_time = time.time() - start
l_currentA /= 100.0
r_currentA /= 100.0
if crc == 1:
# print("M1 moter current "+'{:.3}'.format(l_current)+"[A]")
# print("M2 moter current "+'{:.3}'.format(r_current)+"[A]")
print("elapsed_time:{0}".format(elapsed_time) + "[sec] " +
"M1 moter current " + '{:.3}'.format(l_currentA) + "[A]")
print("elapsed_time:{0}".format(elapsed_time) + "[sec] " +
"M2 moter current " + '{:.3}'.format(r_currentA) + "[A]")
else:
print("Failed")