def __init__(self): # Create motor serial object self._robot_x = 0 self._robot_y = 0 self._robot_yaw = 0 self._pose_cov = [ 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0. ] self._use_all_sens = True self.motor_serial = imrt_robot_serial.IMRTRobotSerial() rospy.Subscriber("teleop/cmd_vel", Twist, self.cmd_callback) self.scan_pub = rospy.Publisher('sonics_scan', LaserScan, queue_size=1) self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=1) self._tf_broadcaster = tf.TransformBroadcaster() # Open serial port. Exit if serial port cannot be opened try: self.motor_serial.connect("/dev/ttyACM0") except: rospy.loginfo( "Could not open port. Is your robot connected?\nExiting program" ) sys.exit() # Start serial receive thread self.motor_serial.run() rospy.loginfo("Running..")
def main(): vx_gain = 2 wz_gain = 4 controller = imrt_xbox.IMRTxbox() # Create motor serial object motor_serial = imrt_robot_serial.IMRTRobotSerial() # Open serial port. Exit if serial port cannot be opened try: motor_serial.connect("/dev/ttyACM0") except: print("Could not open port. Is your robot connected?\nExiting program") sys.exit() # Start serial receive thread motor_serial.run() try: while not motor_serial.shutdown_now: but_a = controller.get_a() but_b = controller.get_b() but_x = controller.get_x() but_y = controller.get_y() ax_lx = controller.get_left_x() ax_ly = controller.get_left_y() ax_rx = controller.get_right_x() ax_ry = controller.get_right_y() # use pos.x, pos.y and pos.distance to determin vx and wz vx = vx_gain * ax_ly #* (-1,1)[bd.position.y > 0] wz = -wz_gain * ax_rx #* (1,-1)[bd.position.y > 0] print(vx, wz) # calculate motor commands v1 = (vx - ROBOT_WIDTH * wz / 2) * 200 v2 = (vx + ROBOT_WIDTH * wz / 2) * 200 print("HEI") # send motor commands motor_serial.send_command(int(v1), int(v2)) #print("a: {}, b: {}, x: {}, y: {}, lx: {:+.2f}, ly: {:+.2f}, rx: {:+.2f}, ry: {:+.2f}".format(but_a, but_b, but_x, but_y, ax_lx, ax_ly, ax_rx, ax_ry), end='\r') time.sleep(0.1) finally: controller.shutdown() print("Exiting program")
from __future__ import print_function import imrt_xbox import imrt_robot_serial import sys import signal import time motor_serial = imrt_robot_serial.IMRTRobotSerial() try: motor_serial.connect("/dev/ttyACM0") except: print("Could not open port. Is your robot connected?\nExiting program") sys.exit() motor_serial.run() # Format floating point number to string format -x.xxx def fmtFloat(n): return '{:6.3f}'.format(n) # Print one or more values without a line feed def show(*args): for arg in args: print(arg, end="") # Print true or false value based on a boolean, without linefeed def showIf(boolean, ifTrue, ifFalse=" "): if boolean: show(ifTrue) else: