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")
try:
roboclaw.ForwardM1(self.address_front, 0)
roboclaw.ForwardM2(self.address_front, 0)
roboclaw.ForwardM1(self.address_back, 0)
roboclaw.ForwardM2(self.address_back, 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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address_front)
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)
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)
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)
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()
r_time.sleep()
def displayspeed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
print("Encoder1:"),
if(enc1[0]==1):
print enc1[1],
print format(enc1[2],'02x'),
else:
print "failed",
print "Encoder2:",
if(enc2[0]==1):
print enc2[1],
print format(enc2[2],'02x'),
else:
print "failed " ,
print "Speed1:",
if(speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if(speed2[0]):
print speed2[1]
else:
print "failed "
def encoders(self):
Enc1 = roboclaw.ReadEncM1(self.address)
Enc2 = roboclaw.ReadEncM2(self.address)
#enc_R=float(-Enc2[1])
#enc_L=float(Enc1[1])
enc_L = float(Enc2[1])
enc_R = float(Enc1[1])
speed1 = roboclaw.ReadSpeedM1(self.address)
speed1 = float(speed1[1])
speed2 = roboclaw.ReadSpeedM2(self.address)
speed2 = float(speed2[1])
encoder = "LEFT ENC: " + str(enc_L) + ", RIGHT ENC: " + str(enc_R)
self.encoder_pub.publish(encoder)
Wr = (0.0092 * speed1) - 0.1514 #RPM_R
Wl = (0.0092 * speed2) + 0.0126 #RPM_L
self.WR = ((2 * pi) / 60) * Wr #rad/s R
self.WL = ((2 * pi) / 60) * Wl #rad/s L
#speedM= "Speed_L: "+str(self.WL)+", Speed_R: "+str(self.WR)+" [rad/s]"
WL = str(self.WL)
WR = str(self.WR)
RefL = str(self.ref_WL)
RefR = str(self.ref_WR)
#self.speed_pub.publish(speedM)
self.WL_pub.publish(WL)
self.WR_pub.publish(WR)
self.RefL_pub.publish(RefL)
self.RefR_pub.publish(RefR)
def run(self):
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
rospy.logdebug("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
try:
status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
except ValueError:
rospy.logwarn("Value Error passed M1")
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:
rospy.logwarn("Value Error passed M2")
pass
except OSError as e:
rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
#if (enc1 in locals()) and (enc2 in locals()):
try:
if (g_invert_motor_axes):
enc1 = -enc1
enc2 = -enc2
if (g_flip_left_right_motors):
enc1_t = enc1
enc2_t = enc2
enc2 = enc1_t
enc1 = enc2_t
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(
enc2, enc1) #update_publish expects enc_left enc_right
#self.updater.update()
except:
print("problems reading encoders")
r_time.sleep()
def pub_enc_values(self):
pitch_state = JointState()
pitch_state.header = Header()
pitch_state.header.stamp = rospy.Time.now()
pitch_state.name = ['m1', 'm2']
enc1 = roboclaw.ReadEncM1(self.address)
enc2 = roboclaw.ReadEncM2(self.address)
pitch_state.position = [float(enc1[1]),float(enc2[1])]
self.pitch_pub.publish(pitch_state)
self.last_pub_time = rospy.get_rostime()
def pub_enc_values(self):
height_state = JointState()
height_state.header = Header()
height_state.header.stamp = rospy.Time.now()
height_state.name = ['m1', 'm2']
enc1 = roboclaw.ReadEncM1(self.address)
enc2 = roboclaw.ReadEncM2(self.address)
height_state.position = [float(enc1[1]),float(enc2[1])]
try:
self.height_pub.publish(height_state)
except Exception as e:
rospy.loginfo("hanging")
rospy.logdebug(e)
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 run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if not self.stopped and (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, 0)
roboclaw.ForwardM2(self.address, 0)
self.stopped = True
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
else:
if self.twist != None:
self.go_twist(self.twist)
# 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 != None and enc2 != None):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
if(self.LEFT_MOTOR_NUMBER == 1):
enc_left = enc1
enc_right = enc2
else:
enc_left = enc2
enc_right = enc1
self.encodm.update_publish(enc_left, enc_right)
self.updater.update()
r_time.sleep()
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")
try:
# roboclaw.ForwardM1(self.address, 0)
# roboclaw.ForwardM2(self.address, 0)
with self.mutex:
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
try:
# status1, enc1, crc1 = roboclaw.ReadEncM1(self.address)
with self.mutex:
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)
with self.mutex:
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()):
if (isinstance(enc1, Number) and isinstance(enc2, Number)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
# self.encodm.update_publish(enc1, enc2)
self.encodm.update_publish(enc2, enc1)
self.updater.update()
r_time.sleep()
def displayspeed():
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
list = [
str(enc1[1]) + ",",
str(enc2[1]) + ",",
str(speed1[1]) + ",",
str(speed2[1])
]
print("Encoder1:"),
if (enc1[0] == 1):
print enc1[1],
print format(enc1[2], '02x'),
else:
print "failed",
list[0] = "failed"
print "Encoder2:",
if (enc2[0] == 1):
print enc2[1],
print format(enc2[2], '02x'),
else:
print "failed ",
list[1] = "failed"
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
list[2] = "failed"
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed"
list[3] = "failed"
file.writelines(list)
file.write('\n')
def run(self):
rospy.loginfo("Starting motor drive")
r_time = rospy.Rate(30)
while not rospy.is_shutdown():
# stop movement if robot doesn't recieve commands for 1 sec
if (self.STOP_MOVEMENT and not self.movement.stopped
and rospy.get_rostime().to_sec() -
self.movement.last_set_speed_time.to_sec() > 1):
rospy.loginfo("Did not get command 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)
self.movement.stopped = True
# 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() and enc1 and enc2):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
if (self.encodm):
self.encodm.update_publish(enc1, enc2)
self.updater.update()
self.movement.run()
r_time.sleep()
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting motor drive")
roboclaw.Flush()
r_time = rospy.Rate(10)
while not rospy.is_shutdown():
if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > self.TIMEOUT:
rospy.loginfo("Did not get command for 1 second, stopping")
try:
roboclaw.ForwardM1(self.address, 0)
roboclaw.ForwardM2(self.address, 0)
roboclaw.SpeedM1M2(self.address, 0, 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
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 is not None) and (enc2 is not None)):
rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
self.encodm.update_publish(enc1, enc2)
self.updater.update()
r_time.sleep()
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")
try:
roboclaw.ForwardM1(self.rear_address, 0)
roboclaw.ForwardM2(self.rear_address, 0)
roboclaw.ForwardM1(self.front_address, 0)
roboclaw.ForwardM2(self.front_address, 0)
self.last_set_speed_time = rospy.get_rostime()
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
rear_status1, rear_enc1, rear_crc1 = None, None, None
rear_status2, rear_enc2, rear_crc2 = None, None, None
front_status1, front_enc1, front_crc1 = None, None, None
front_status2, front_enc2, front_crc2 = None, None, None
# rear roboclaw
try:
rear_status1, rear_enc1, rear_crc1 = roboclaw.ReadEncM1(
self.rear_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Rear ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
rear_status2, rear_enc2, rear_crc2 = roboclaw.ReadEncM2(
self.rear_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Rear ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
# front roboclaw
try:
front_status1, front_enc1, front_crc1 = roboclaw.ReadEncM1(
self.front_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Front ReadEncM1 OSError: %d", e.errno)
rospy.logdebug(e)
try:
front_status2, front_enc2, front_crc2 = roboclaw.ReadEncM2(
self.front_address)
except ValueError:
pass
except OSError as e:
rospy.logwarn("Front ReadEncM2 OSError: %d", e.errno)
rospy.logdebug(e)
if (rear_enc1 is not None and rear_enc2 is not None
and front_enc1 is not None and front_enc2 is not None):
rospy.logdebug(" Encoders rear: %s %s front: %s %s",
rear_enc1, rear_enc2, front_enc1, front_enc2)
self.encodm.update_publish(rear_enc1, rear_enc2, front_enc1,
front_enc2)
if (self.DIAGNOSTIC):
self.updater.update()
r_time.sleep()
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()
