def shutdown(self):
global p1,p2
rospy.loginfo("Shutting down")
try:
roboclaw.ForwardBackwardM1(self.address_front, 63,p1)
roboclaw.ForwardBackwardM2(self.address_front, 63,p1)
roboclaw.ForwardBackwardM1(self.address_back, 63,p2)
roboclaw.ForwardBackwardM2(self.address_back, 63,p2)
except OSError:
rospy.logerr("Shutdown did not work trying again")
try:
roboclaw.ForwardBackwardM1(self.address_front, 63,p1)
roboclaw.ForwardBackwardM2(self.address_front, 63,p1)
roboclaw.ForwardBackwardM1(self.address_back, 63,p2)
roboclaw.ForwardBackwardM2(self.address_back, 63,p2)
except OSError as e:
rospy.logerr("Could not shutdown motors!!!!")
rospy.logdebug(e)
def animate(i):
global p1,p2,get_cmd_vel,U, wheel_pwm_err_prev, wheel_pwm_int_err, xvals, yvals1, yvals2
max_possible_speed=28.19 #- angular vel - rad/s
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address_front, 0,p1)
roboclaw.ForwardM2(self.address_front, 0,p1)
roboclaw.ForwardM1(self.address_back, 0,p2)
roboclaw.ForwardM2(self.address_back, 0,p2)
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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address_front,p1)
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_front,p1)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status3, enc3, crc3 = roboclaw.ReadEncM1(self.address_back,p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM3 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status4, enc4, crc4 = roboclaw.ReadEncM2(self.address_back,p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM4 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars()) and ('enc3' in vars()) and ('enc4' in vars()):
rospy.logdebug(" Encoders %d %d %d %d" % (enc1, enc2,enc3,enc4))
W, d_time = self.encodm.update_publish(enc1, enc2, enc3, enc4)
# self.updater.update()
# rospy.logwarn("wheel velocities %f %f %f %f " %(U[0],U[1],U[2],U[3]))
# rospy.logwarn("W is %f %f %f %f " %(W[0],W[1],W[2],W[3]))
# rospy.logwarn("d_time is %f" %d_time)
# if(get_cmd_vel==1):
try:
if U[0] is 0 and U[1] is 0 and U[2] is 0 and U[3] is 0:
roboclaw.ForwardBackwardM1(self.address_front, 63,p1)
roboclaw.ForwardBackwardM2(self.address_front, 63,p1)
roboclaw.ForwardBackwardM1(self.address_back, 63,p2)
roboclaw.ForwardBackwardM2(self.address_back, 63,p2)
else:
wheel_pwm_err = U-W
wheel_pwm_deriv_err = (wheel_pwm_err-wheel_pwm_err_prev)/d_time
wheel_pwm_int_err = wheel_pwm_int_err + wheel_pwm_err * d_time
rospy.logwarn(" wheel_pwm_err %f %f %f %f " %(wheel_pwm_err[0],wheel_pwm_err[1],wheel_pwm_err[2],wheel_pwm_err[3]))
rospy.logwarn(" wheel_pwm_deriv_err %f %f %f %f " %(wheel_pwm_deriv_err[0],wheel_pwm_deriv_err[1],wheel_pwm_deriv_err[2],wheel_pwm_deriv_err[3]))
rospy.logwarn(" wheel_pwm_int_err %f %f %f %f " %(wheel_pwm_int_err[0],wheel_pwm_int_err[1],wheel_pwm_int_err[2],wheel_pwm_int_err[3]))
Kp_err = 0.5
Kd_err = 0.001
Ki_err = 0.01
wheel_pwm_acc = Kp_err*wheel_pwm_err+Kd_err*wheel_pwm_deriv_err + Ki_err*wheel_pwm_int_err
wheel_pwm_err_prev = wheel_pwm_err
rospy.logwarn(" wheel_pwm_acc %f %f %f %f " %(wheel_pwm_acc[0],wheel_pwm_acc[1],wheel_pwm_acc[2],wheel_pwm_acc[3]))
################plotting
xvals.append(next(index))
yvals1.append(W[0])
yvals2.append(U[0])
plt.cla()
plt.plot(xvals,yvals1, label='current wheel vel')
plt.plot(xvals,yvals2, label='desired wheel vel')
plt.legend(loc='upper left')
# real_sp_m1 = ((wheel_pwm_acc[0]/max_possible_speed)*63)+64
# real_sp_m2 = ((wheel_pwm_acc[1]/max_possible_speed)*63)+64
# real_sp_m3 = ((wheel_pwm_acc[2]/max_possible_speed)*63)+64
# real_sp_m4 = ((wheel_pwm_acc[3]/max_possible_speed)*63)+64
real_sp_m1 = ((U[0]/max_possible_speed)*63)+64
real_sp_m2 = ((U[1]/max_possible_speed)*63)+64
real_sp_m3 = ((U[2]/max_possible_speed)*63)+64
real_sp_m4 = ((U[3]/max_possible_speed)*63)+64
roboclaw.ForwardBackwardM1(self.address_front, int(real_sp_m1),p1)
roboclaw.ForwardBackwardM2(self.address_front, int(real_sp_m2),p1)
roboclaw.ForwardBackwardM1(self.address_back, int(real_sp_m3),p2)
roboclaw.ForwardBackwardM2(self.address_back, int(real_sp_m4),p2)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
# get_cmd_vel=0
r_time.sleep()
def run(self):
global p1, p2, get_cmd_vel, U, wheel_pwm_err_prev, wheel_pwm_int_err
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(100)
max_possible_speed = 28.19 #- angular vel - rad/s
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")
try:
roboclaw.ForwardM1(self.address_front, 0, p1)
roboclaw.ForwardM2(self.address_front, 0, p1)
roboclaw.ForwardM1(self.address_back, 0, p2)
roboclaw.ForwardM2(self.address_back, 0, p2)
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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(
self.address_front, p1)
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_front, p1)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status3, enc3, crc3 = roboclaw.ReadEncM1(self.address_back, p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM3 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status4, enc4, crc4 = roboclaw.ReadEncM2(self.address_back, p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM4 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars()) and (
'enc3' in vars()) and ('enc4' in vars()):
rospy.logdebug(" Encoders %d %d %d %d" %
(enc1, enc2, enc3, enc4))
W, d_time = self.encodm.update_publish(enc1, enc2, enc3, enc4)
# self.updater.update()
rospy.logwarn("wheel velocities %f %f %f %f " %
(U[0], U[1], U[2], U[3]))
rospy.logwarn("W is %f %f %f %f " % (W[0], W[1], W[2], W[3]))
rospy.logwarn("d_time is %f" % d_time)
# if(get_cmd_vel==1):
try:
if U[0] is 0 and U[1] is 0 and U[2] is 0 and U[3] is 0:
roboclaw.ForwardBackwardM1(self.address_front, 63, p1)
roboclaw.ForwardBackwardM2(self.address_front, 63, p1)
roboclaw.ForwardBackwardM1(self.address_back, 63, p2)
roboclaw.ForwardBackwardM2(self.address_back, 63, p2)
else:
wheel_pwm_err = U - W
wheel_pwm_deriv_err = (wheel_pwm_err -
wheel_pwm_err_prev) / d_time
wheel_pwm_int_err = wheel_pwm_int_err + wheel_pwm_err * d_time
rospy.logwarn(wheel_pwm_err)
Kp_err = 0.5
Kd_err = 0.001
Ki_err = 0.01
wheel_pwm_acc = Kp_err * wheel_pwm_err + Kd_err * wheel_pwm_deriv_err + Ki_err * wheel_pwm_int_err
wheel_pwm_err_prev = wheel_pwm_err
rospy.logwarn(wheel_pwm_acc)
real_sp_m1 = (
(wheel_pwm_acc[0] / max_possible_speed) * 63) + 64
real_sp_m2 = (
(wheel_pwm_acc[1] / max_possible_speed) * 63) + 64
real_sp_m3 = (
(wheel_pwm_acc[2] / max_possible_speed) * 63) + 64
real_sp_m4 = (
(wheel_pwm_acc[3] / max_possible_speed) * 63) + 64
# real_sp_m1 = ((U[0]/max_possible_speed)*63)+64
# real_sp_m2 = ((U[1]/max_possible_speed)*63)+64
# real_sp_m3 = ((U[2]/max_possible_speed)*63)+64
# real_sp_m4 = ((U[3]/max_possible_speed)*63)+64
roboclaw.ForwardBackwardM1(self.address_front,
int(real_sp_m1), p1)
roboclaw.ForwardBackwardM2(self.address_front,
int(real_sp_m2), p1)
roboclaw.ForwardBackwardM1(self.address_back,
int(real_sp_m3), p2)
roboclaw.ForwardBackwardM2(self.address_back,
int(real_sp_m4), p2)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
# get_cmd_vel=0
r_time.sleep()
def run(self):
global p1, p2, get_cmd_vel, U
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(100)
max_possible_speed = 1.79
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")
try:
roboclaw.ForwardM1(self.address_front, 0, p1)
roboclaw.ForwardM2(self.address_front, 0, p1)
roboclaw.ForwardM1(self.address_back, 0, p2)
roboclaw.ForwardM2(self.address_back, 0, p2)
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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(
self.address_front, p1)
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_front, p1)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status3, enc3, crc3 = roboclaw.ReadEncM1(self.address_back, p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM3 OSError: %d", e.errno)
rospy.logdebug(e)
try:
status4, enc4, crc4 = roboclaw.ReadEncM2(self.address_back, p2)
except ValueError:
pass
except OSError as e:
rospy.logwarn("ReadEncM4 OSError: %d", e.errno)
rospy.logdebug(e)
if ('enc1' in vars()) and ('enc2' in vars()) and (
'enc3' in vars()) and ('enc4' in vars()):
rospy.logdebug(" Encoders %d %d %d %d" %
(enc1, enc2, enc3, enc4))
self.encodm.update_publish(enc1, enc2, enc3, enc4)
# self.updater.update()
if (get_cmd_vel == 1):
try:
if U[0] is 0 and U[1] is 0 and U[2] is 0 and U[3] is 0:
roboclaw.ForwardBackwardM1(self.address_front, 63, p1)
roboclaw.ForwardBackwardM2(self.address_front, 63, p1)
roboclaw.ForwardBackwardM1(self.address_back, 63, p2)
roboclaw.ForwardBackwardM2(self.address_back, 63, p2)
else:
rospy.logwarn("wheel velocities %f %f %f %f " %
(U[0], U[1], U[2], U[3]))
real_sp_m1 = ((U[0] / max_possible_speed) * 63) + 64
real_sp_m2 = ((U[1] / max_possible_speed) * 63) + 64
real_sp_m3 = ((U[2] / max_possible_speed) * 63) + 64
real_sp_m4 = ((U[3] / max_possible_speed) * 63) + 64
roboclaw.ForwardBackwardM1(self.address_front,
int(real_sp_m1), p1)
roboclaw.ForwardBackwardM2(self.address_front,
int(real_sp_m2), p1)
roboclaw.ForwardBackwardM1(self.address_back,
int(real_sp_m3), p2)
roboclaw.ForwardBackwardM2(self.address_back,
int(real_sp_m4), p2)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)
get_cmd_vel = 0
r_time.sleep()
def cmd_vel_callback(self, twist):
global p1, p2
self.last_set_speed_time = rospy.get_rostime()
max_linear_speed = 0.5
max_angular_speed = 0.5
max_possible_speed = 1.79
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.00, -1.00, -(lx + ly)], [1.00, 1.00, (lx + ly)],
[1.00, 1.00, -(lx + ly)], [1.00, -1.00, (lx + ly)]])
X[0] = linear_x
X[1] = linear_y
X[2] = angular_z
U_inv = X
#wheel velocities
U = np.dot(B_inv, U_inv)
rospy.logwarn("wheel velocities %f %f %f %f " %
(U[0], U[1], U[2], U[3]))
try:
if U[0] is 0 and U[1] is 0 and U[2] is 0 and U[3] is 0:
roboclaw.ForwardBackwardM1(self.address_front, 63, p1)
roboclaw.ForwardBackwardM2(self.address_front, 63, p1)
roboclaw.ForwardBackwardM1(self.address_back, 63, p2)
roboclaw.ForwardBackwardM2(self.address_back, 63, p2)
else:
real_sp_m1 = ((U[0] / max_possible_speed) * 63) + 64
real_sp_m2 = ((U[1] / max_possible_speed) * 63) + 64
real_sp_m3 = ((U[2] / max_possible_speed) * 63) + 64
real_sp_m4 = ((U[3] / max_possible_speed) * 63) + 64
roboclaw.ForwardBackwardM1(self.address_front, int(real_sp_m1),
p1)
roboclaw.ForwardBackwardM2(self.address_front, int(real_sp_m2),
p1)
roboclaw.ForwardBackwardM1(self.address_back, int(real_sp_m3),
p2)
roboclaw.ForwardBackwardM2(self.address_back, int(real_sp_m4),
p2)
except OSError as e:
rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
rospy.logdebug(e)