def update(self):
for i, pub in enumerate(self.enc_publishers):
if pub is None:
continue
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
response = roboclaw.ReadEncM1(address)
elif self.parameters[i]['motor_num'] == 2:
response = roboclaw.ReadEncM2(address)
else:
continue
msg = Int32()
msg.data = int(response[1])
pub.publish(msg)
for i, t in enumerate(self.timeouts):
if self.enc_publishers[i] is None:
continue
if int(round(time.time() * 1000)) - t > self.TIMEOUT_TIME:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, 64)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, 64)
else:
continue
'''
for i, pwm in enumerate(self.pwms):
if pwm is None:
continue
if self.parameters[i] is not None:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, pwm)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, pwm)
self.pwms[i] = None
'''
for i, vel in enumerate(self.velocities):
if vel is None:
continue
#print('found_vel: ' + str(vel))
if self.parameters[i] is not None:
address = self.parameters[i]['address']
#print('address: ' + str(address))
if self.parameters[i]['motor_num'] == 1:
#print('setting m1 speed')
if roboclaw.SpeedM1(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
elif self.parameters[i]['motor_num'] == 2:
#print('setting m2 speed')
if roboclaw.SpeedM2(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
self.velocities[i] = None
'''
import time
import roboclaw
#Windows comport name
roboclaw.Open("COM3", 115200)
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
while (1):
roboclaw.ForwardM1(address, 64)
roboclaw.BackwardM2(address, 64)
time.sleep(2)
roboclaw.BackwardM1(address, 64)
roboclaw.ForwardM2(address, 64)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 96)
roboclaw.ForwardBackwardM2(address, 32)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 32)
roboclaw.ForwardBackwardM2(address, 96)
time.sleep(2)
def update(self):
"""
Sends
"""
signal.setitimer(
signal.ITIMER_REAL, 0.25
) # set the watchdog for 0.25 seconds (SIGALARM_handler() will be
# called if this function takes too long)
try:
# Publish encoder readings
if self.m1_enc_pub is not None:
response_1 = roboclaw.ReadEncM1(self.address)
response_2 = roboclaw.ReadEncM2(self.address)
if response_1[0] == 0 or response_1[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from encoder reading: " +
str(response_1) + " " + str(response_2))
else:
m1_msg = Int32()
m1_msg.data = int(response_1[1])
m2_msg = Int32()
m2_msg.data = int(response_2[1])
self.m1_enc_pub.publish(m1_msg)
self.m2_enc_pub.publish(m2_msg)
# Publish current readings
if self.m1_enc_pub is not None:
currents = roboclaw.ReadCurrents(self.address)
if currents[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from current reading: " +
str(currents))
else:
m1_amp_msg = Float32()
m1_amp_msg.data = currents[1] / 100.0
m2_amp_msg = Float32()
m2_amp_msg.data = currents[2] / 100.0
self.m1_amp_pub.publish(m1_amp_msg)
self.m2_amp_pub.publish(m2_amp_msg)
bat_voltage = roboclaw.ReadMainBatteryVoltage(self.address)
if bat_voltage[0] == 0:
rospy.logerr(
str((self.name, os.getpid())) +
": error returned from battery voltage reading: " +
str(bat_voltage))
else:
bat_volt_msg = Float32()
bat_volt_msg.data = bat_voltage[1] / 10.0
self.battery_voltage_pub.publish(bat_volt_msg)
# Timeout if no command recieved for more than TIMEOUT_TIME
if int(round(
time.time() * 1000)) - self.timeout > self.TIMEOUT_TIME:
roboclaw.ForwardBackwardM1(self.address, 64)
roboclaw.ForwardBackwardM2(self.address, 64)
else:
# PWM control
if self.m1_pwm is not None:
roboclaw.ForwardBackwardM1(self.address, self.m1_pwm)
# roboclaw.DutyM1(self.address, (self.m1_pwm - 64) * int(32767 / 64))
self.m1_pwm = None
if self.m2_pwm is not None:
roboclaw.ForwardBackwardM2(self.address, self.m2_pwm)
# roboclaw.DutyM2(self.address, (self.m2_pwm - 64) * int(32768 / 64))
self.m2_pwm = None
# Velocity control
if self.m1_vel is not None:
# if the commanded velocity is 0 then turn the motors off instead of sending a zero speed.
# This is to keep the motors from overheating, we once left the rover still on a slight slope for
# a few minutes and the motors overheated trying to keep the rover still.
if self.m1_vel == 0:
roboclaw.DutyM1(self.address, 0)
else:
roboclaw.SpeedM1(self.address, self.m1_vel)
self.m1_vel = None
if self.m2_vel is not None:
if self.m2_vel == 0:
roboclaw.DutyM2(self.address, 0)
else:
roboclaw.SpeedM2(self.address, self.m2_vel)
self.m2_vel = None
# Position control
if self.m1_pos is not None:
roboclaw.SpeedAccelDeccelPositionM1(
self.address, 0, self.QPPS, 0, self.m1_pos, False)
self.m1_pos = None
if self.m2_pos is not None:
roboclaw.SpeedAccelDeccelPositionM2(
self.address, 0, self.QPPS, 0, self.m2_pos, False)
self.m2_pos = None
# this exception will be raised by the SIGALRM handler if sending/recieving data from the roboclaw took too long
except Exception as e:
rospy.logerr("SIGLARAM: " + str((self.name, os.getpid())) + ": " +
str(e))