def cmd_pwm_callback(self, point):
self.last_set_speed_time = rospy.get_rostime()
motor1 = int(point.x)
motor2 = int(point.y)
# rospy.logdebug("motor1:%d motor2: %d", motor1, motor2)
try:
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
if motor1 is 0 and motor2 is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
if (motor1 >= 0):
roboclaw.ForwardM1(self.address, motor1)
else:
roboclaw.BackwardM1(self.address, -motor1)
if (motor2 >= 0):
roboclaw.ForwardM2(self.address, motor2)
else:
roboclaw.BackwardM2(self.address, -motor2)
except OSError as e:
rospy.logwarn("Motor OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s
vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
rospy.loginfo("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
if vr_ticks is 0 and vl_ticks is 0:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
else:
if 0 <= vr_ticks:
roboclaw.ForwardM1(self.address, vr_ticks)
else:
roboclaw.BackwardM1(self.address, -vr_ticks)
if 0 <= vl_ticks:
roboclaw.ForwardM2(self.address, vl_ticks)
else:
roboclaw.BackwardM2(self.address, -vl_ticks)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
self.last_set_speed_time = rospy.get_rostime()
linear_x = twist.linear.x
if linear_x > self.MAX_SPEED:
linear_x = self.MAX_SPEED
if linear_x < -self.MAX_SPEED:
linear_x = -self.MAX_SPEED
rospy.logdebug("linear_x before:%f, after:%f", twist.linear.x,
linear_x)
# velocity in m/s
v_l = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
v_r = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0
rospy.logdebug("angular.z:%f, base_width:%f, v_l:%f, v_r:%f",
twist.angular.z, self.BASE_WIDTH, v_l, v_r)
# roboclaw duty values
rc_val_l = int(self.MAX_COMMAND_VALUE / self.MAX_SPEED * v_l)
rc_val_r = int(self.MAX_COMMAND_VALUE / self.MAX_SPEED * v_r)
rospy.logdebug("rc_val_l before:%f, rc_val_r before:%f", rc_val_l,
rc_val_r)
if rc_val_l > self.MAX_COMMAND_VALUE:
rc_val_l = self.MAX_COMMAND_VALUE
if rc_val_l < -self.MAX_COMMAND_VALUE:
rc_val_l = -self.MAX_COMMAND_VALUE
if rc_val_r > self.MAX_COMMAND_VALUE:
rc_val_r = self.MAX_COMMAND_VALUE
if rc_val_r < -self.MAX_COMMAND_VALUE:
rc_val_r = -self.MAX_COMMAND_VALUE
rospy.logdebug("rc_val_l after:%f, rc_val_r after:%f", rc_val_l,
rc_val_r)
try:
if rc_val_r >= 0:
roboclaw.ForwardM1(self.address, rc_val_r)
else:
roboclaw.BackwardM1(self.address, rc_val_r * -1)
if rc_val_l >= 0:
roboclaw.ForwardM2(self.address, rc_val_l)
else:
roboclaw.BackwardM2(self.address, rc_val_l * -1)
except OSError as e:
rospy.logwarn("ForwardM1/M2 OSError: %d", e.errno)
rospy.logdebug(e)
def cmd_vel_callback(self, twist):
"""Command the motors based on the incoming twist message"""
self.last_set_speed_time = rospy.get_rostime()
rospy.logdebug("Twist: -Linear X: %d -Angular Z: %d",twist.linear.x,twist.angular.z)
linear_x = twist.linear.x
angular_z = twist.angular.z
if linear_x > self.LINEAR_MAX_SPEED:
linear_x = self.LINEAR_MAX_SPEED
elif linear_x < -self.LINEAR_MAX_SPEED:
linear_x = -self.LINEAR_MAX_SPEED
# Take linear x and angular z values and compute command
motor1_command = linear_x/self.LINEAR_MAX_SPEED + angular_z/self.ANGULAR_MAX_SPEED
motor2_command = linear_x/self.LINEAR_MAX_SPEED - angular_z/self.ANGULAR_MAX_SPEED
# Scale to motor pwm
motor1_command = int(motor1_command * 127)
motor2_command = int(motor2_command * 127)
# Clip commands to within bounds (-127,127)
motor1_command = min(127, motor1_command)
motor1_command = max(-127, motor1_command)
motor2_command = min(127, motor2_command)
motor2_command = max(-127, motor2_command)
rospy.logdebug("motor1 command = %d",int(motor1_command))
rospy.logdebug("motor2 command = %d",int(motor2_command))
try:
if motor1_command >= 0:
roboclaw.ForwardM1(self.address, motor1_command)
else:
roboclaw.BackwardM1(self.address, -motor1_command)
if motor2_command >= 0:
roboclaw.ForwardM2(self.address, motor2_command)
else:
roboclaw.BackwardM2(self.address, -motor2_command)
except OSError as e:
rospy.logwarn("Roboclaw OSError: %d", e.errno)
rospy.logdebug(e)
import time
import roboclaw_driver.roboclaw_driver as rc
#Windows comport name
#rc = Roboclaw("COM9",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open("/dev/ttyACM0", 115200)
address = 0x80
while(1):
rc.ForwardM1(address,32) #1/4 power forward
rc.BackwardM2(address,32) #1/4 power backward
time.sleep(2)
rc.BackwardM1(address,32) #1/4 power backward
rc.ForwardM2(address,32) #1/4 power forward
time.sleep(2)
rc.BackwardM1(address,0) #Stopped
rc.ForwardM2(address,0) #Stopped
time.sleep(5)
m1duty = 16
m2duty = -16
rc.ForwardBackwardM1(address,64+m1duty) #1/4 power forward
rc.ForwardBackwardM2(address,64+m2duty) #1/4 power backward
time.sleep(2)
m1duty = -16