def cmd_vel_callback(self, twist):
global avg_speed
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
# rospy.logwarn("linear_x:%d twist.angular.z: %d base_width %d", linear_x, twist.angular.z,self.BASE_WIDTH)
# rospy.logwarn("vr:%d vl: %d", vr, vl)
vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s
vl_ticks = int(vl * self.TICKS_PER_METER)
# rospy.logwarn("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
try:
real_sp_m1 = ((linear_x / 1.7) * 63) + 64
roboclaw.ForwardBackwardM1(self.address, int(real_sp_m1))
roboclaw.ForwardBackwardM2(self.address, int(real_sp_m1))
# speed1 = roboclaw.ReadEncM1(self.address)
# rospy.logwarn("speed M1")
# This is a hack way to keep a poorly tuned PID from making noise at speed 0
# rospy.logwarn("speed M1")
# sm1 = roboclaw.ReadSpeedM1(self.address)
# rospy.logwarn(sm1)
# rospy.logwarn("speed M2")
# sm2 = roboclaw.ReadSpeedM2(self.address)
# rospy.logwarn(sm2)
# if vr_ticks is 0 and vl_ticks is 0:
# roboclaw.ForwardM1(self.address, 0)
# roboclaw.ForwardM2(self.address, 0)
# rospy.logwarn("forward")
# else:
# roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
# rospy.logwarn("speed")
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()
max_linear_speed = 1.7
max_angular_speed = 1.7
lx = self.BASE_LENGTH
ly = self.BASE_WIDTH
r = self.WHEEL_RADIUS
linear_x = twist.linear.x
linear_y = twist.linear.y
angular_z = twist.angular.z
if linear_x > max_linear_speed:
linear_x = max_linear_speed
if linear_x < -max_linear_speed:
linear_x = -max_linear_speed
if linear_y > max_linear_speed:
linear_y = max_linear_speed
if linear_y < -max_linear_speed:
linear_y = -max_linear_speed
if angular_z > max_angular_speed:
angular_z = max_angular_speed
if angular_z < -max_angular_speed:
angular_z = -max_angular_speed
#######inverse kinematic Equations
B_inv = np.array([[1, -1, -(lx + ly) / 2], [1, 1, (lx + ly) / 2],
[1, 1, -(lx + ly) / 2], [1, -1, (lx + ly) / 2]])
X[0] = linear_x
X[1] = linear_y
X[2] = angular_z
U_inv = X
#wheel velocities
U = np.dot(B_inv, U_inv)
try:
if U[0] is 0 and U[1] is 0:
roboclaw.ForwardM1(self.address_front, 0)
roboclaw.ForwardM2(self.address_front, 0)
roboclaw.ForwardM1(self.address_back, 0)
roboclaw.ForwardM2(self.address_back, 0)
else:
real_sp_m1 = ((U[0] / 1.7) * 63) + 64
real_sp_m2 = ((U[1] / 1.7) * 63) + 64
real_sp_m3 = ((U[2] / 1.7) * 63) + 64
real_sp_m4 = ((U[3] / 1.7) * 63) + 64
roboclaw.ForwardBackwardM1(self.address_front, int(real_sp_m1))
roboclaw.ForwardBackwardM2(self.address_front, int(real_sp_m2))
roboclaw.ForwardBackwardM1(self.address_back, int(real_sp_m3))
roboclaw.ForwardBackwardM2(self.address_back, int(real_sp_m4))
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
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 m2duty = 16 rc.ForwardBackwardM1(address,64+m1duty) #1/4 power backward rc.ForwardBackwardM2(address,64+m2duty) #1/4 power forward time.sleep(2) rc.ForwardBackwardM1(address,64) #Stopped rc.ForwardBackwardM2(address,64) #Stopped time.sleep(5)
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
if self.value == 1:
try:
self.encoders()
UR = self.WR_Control()
UL = self.WL_Control()
pub_pid = "Left ref: " + str(
self.ref_WL) + ", Speed_L: " + str(
self.WL) + ", UL: " + str(
UL) + ", Right ref: " + str(
self.ref_WR) + ", Speed_R: " + str(
self.WR) + ", UR: " + str(UR)
self.pid_pub.publish(pub_pid)
#if abs(self.ref_WR)<0.1:
# UR=0
#if abs(self.ref_WL)<0.1:
# UL=0
roboclaw.ForwardBackwardM2(self.address, int(63 - UR))
roboclaw.ForwardBackwardM1(self.address,
int(63 + UL)) #Left
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
elif self.value == 0:
try:
self.encoders()
roboclaw.ForwardBackwardM1(self.address,
63 + self.last_UR) #Left
roboclaw.ForwardBackwardM2(self.address,
63 - self.last_UL)
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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status2, enc2, crc2 = roboclaw.ReadEncM2(self.address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
#if ('enc1' in vars()) and ('enc2' in vars()):
#rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
#self.encodm.update_publish(enc1, enc2)
#self.updater.update()
r_time.sleep()