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 debug_robot(): rc.Open('/dev/ttyUSB0', 115200) while True: status = rc.ReadError(MTR_ADDRESS)[1] encoder = rc.ReadEncM1(MTR_ADDRESS) print encoder
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()