def __init__(self, target_name, target_addr, bt_addr, port, default_update_rate=50.0): self.update_rate = default_update_rate self.sampling_divisor = int(400/self.update_rate) self.is_connected = False self._init_pubsub() self._init_params() #self.robot = sphero_driver.Sphero(target_name, target_addr) if target_name: if target_addr: if bt_addr: if port>0: self.robot = sphero_driver.Sphero(target_name, target_addr, bt_addr, port) else: self.robot = sphero_driver.Sphero(target_name, target_addr, bt_addr) else: self.robot = sphero_driver.Sphero(target_name, target_addr) else: self.robot = sphero_driver.Sphero(target_name) else: self.robot = sphero_driver.Sphero() self.imu = Imu() self.imu.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.last_cmd_vel_time = rospy.Time.now() self.last_diagnostics_time = rospy.Time.now() self.cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0
def __init__(self, default_update_rate=50.0): rospy.init_node('sphero') self.update_rate = default_update_rate self.sampling_divisor = int(400/self.update_rate) self.is_connected = False self._init_pubsub() self._init_params() self.robot_name = "sphero" self.robot_bt_addr = None if rospy.has_param('/sphero/bt_addr'): self.robot_bt_addr = rospy.get_param('/sphero/bt_addr') print "connect bt_addr " + str(self.robot_bt_addr) if self.robot_bt_addr != None: self.robot = sphero_driver.Sphero(self.robot_name, self.robot_bt_addr) else: self.robot = sphero_driver.Sphero() self.imu = Imu() self.imu.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.last_cmd_vel_time = rospy.Time.now() self.last_diagnostics_time = rospy.Time.now() self.cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0
def __init__(self, default_update_rate=50.0): rospy.init_node('sphero') self.update_rate = default_update_rate self.sampling_divisor = int(400 / self.update_rate) self.is_connected = False self._init_pubsub() self._init_params() name = rospy.get_param('~name', 'BB') addr = rospy.get_param('~address', None) ble = rospy.get_param('~ble', True) self.robot = sphero_driver.Sphero(name, addr, ble) self.imu = Imu() self.imu.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.angular_velocity_covariance = [ 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 ] self.imu.linear_acceleration_covariance = [ 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 ] self.last_cmd_vel_time = rospy.Time.now() self.last_diagnostics_time = rospy.Time.now() self.cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0
def __init__(self, default_update_rate=50.0): super().__init__('sphero') self.update_rate = default_update_rate self.sampling_divisor = int(400 / self.update_rate) self.is_connected = False self._init_pubsub() self._init_params() self.robot = sphero_driver.Sphero() self.imu = Imu() self.imu.orientation_covariance = [ 1e-6, 0., 0., 0., 1e-6, 0., 0., 0., 1e-6 ] self.imu.angular_velocity_covariance = [ 1e-6, 0., 0., 0., 1e-6, 0., 0., 0., 1e-6 ] self.imu.linear_acceleration_covariance = [ 1e-6, 0., 0., 0., 1e-6, 0., 0., 0., 1e-6 ] self.last_cmd_vel_time = datetime.now() self.last_diagnostics_time = datetime.now() self.cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0
def __init__(self, name, bt_addr, parent, lock): self.robot_name = name self.robot_bt_addr = bt_addr self.parent = parent self.robot = sphero_driver.Sphero(self.robot_name, self.robot_bt_addr, lock) self.robot.setDaemon(True) self.imu = SpheroImu() self.imu.name = name self.imu.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] self.imu.angular_velocity_covariance = [ 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 ] self.imu.linear_acceleration_covariance = [ 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 ] self.last_cmd_vel_time = rospy.Time.now() self.cmd_heading = 0 self.cmd_speed = 0 self.power_state_msg = "No Battery Info" self.power_state = 0 self.is_connected = False
def connectSphero(self, name, address, update_list=False): print "connect " + str(name) + " " + str(address) if self.hasActiveSphero(address) == False: sphero = sphero_driver.Sphero(name, address) sphero.connect() sphero.set_raw_data_strm(40, 1, 0, False) sphero.start() self.sphero_list.append(sphero) if update_list == True: self.spheroMgr.updateList()
#!/usr/bin/python import bluetooth import struct import time from sphero_driver import sphero_driver import sys SPHERO_NAME = "Sphero-RBY" SPHERO_ADDR = "68:86:E7:07:85:E9" sphero = sphero_driver.Sphero() sphero.connect() sphero.set_raw_data_strm(40, 1, 0, False) sphero.start() time.sleep(2) print "disable stabilization" sphero.set_stablization(0, False) time.sleep(3) print "set color to RED" sphero.set_rgb_led(255, 0, 0, 0, False) time.sleep(1) print "set color to GREEN" sphero.set_rgb_led(0, 255, 0, 0, False) time.sleep(1) print "set color to BLUE" sphero.set_rgb_led(0, 0, 255, 0, False) time.sleep(3) print "set back led" sphero.set_rgb_led(255, 255, 255, 0, False) sphero.set_back_led(255, False)
#!/usr/bin/python import bluetooth import struct import time from sphero_driver import sphero_driver import sys sphero = sphero_driver.Sphero(target_name='BB', ble=True) sphero.connect() sphero.set_raw_data_strm(40, 1, 0, False) sphero.start() time.sleep(2) sphero.set_rgb_led(255, 0, 0, 0, False) time.sleep(1) sphero.set_rgb_led(0, 255, 0, 0, False) time.sleep(1) sphero.set_rgb_led(0, 0, 255, 0, False) time.sleep(3) #sphero.join() sphero.disconnect() sys.exit(1)
######################################################################################################################## ######################################################################################################################## # Global helper variables ######################################################################################################################## ######################################################################################################################## # the variables control what happens after a model run restart = True bt_error_restart = False number_of_spheros = len(sphero_addresses) thread_array = [None] * number_of_spheros sphero_array = [None] * number_of_spheros for a, address in enumerate(sphero_addresses): sphero_array[a] = sphero_driver.Sphero("Sphero", address) sphero_internal_headings = [0] * number_of_spheros sphero_target_speeds = [0] * number_of_spheros sphero_current_speeds = [0] * number_of_spheros sphero_target_headings = [0] * number_of_spheros sphero_current_headings = [0] * number_of_spheros first_loop = True first_sphero_boolean = [True] * number_of_spheros first_netlogo_pos = np.zeros((number_of_spheros, 2)) first_sphero_pos = np.zeros((number_of_spheros, 2)) X0_YO = np.zeros((number_of_spheros, 2)) sphero_current_pos = np.zeros((number_of_spheros, 2))
#!/usr/bin/python import bluetooth import struct import time, threading from sphero_driver import sphero_driver import sys SPHERO1_NAME = "Sphero-RBY" SPHERO1_ADDR = "68:86:E7:07:85:E9" SPHERO2_NAME = "Sphero-OGG" SPHERO2_ADDR = "68:86:E7:02:8B:A0" NO_WAIT = True mutual_lock = threading.Lock() sphero1 = sphero_driver.Sphero(SPHERO1_NAME, SPHERO1_ADDR, mutual_lock) sphero2 = sphero_driver.Sphero(SPHERO2_NAME, SPHERO2_ADDR, mutual_lock) sphero1.connect() sphero2.connect() sphero1.set_raw_data_strm(40, 1, 0, False) sphero2.set_raw_data_strm(40, 1, 0, False) sphero1.start() if NO_WAIT == False: time.sleep(2) sphero2.start() if NO_WAIT == False: time.sleep(2) print "disable stabilization" sphero1.set_stablization(0, False) if NO_WAIT == False:
import bluetooth import struct import time from sphero_driver import sphero_driver import sys SPHERO1_NAME = "Sphero-RBY" SPHERO1_ADDR = "68:86:E7:07:85:E9" SPHERO2_NAME = "Sphero-ROB" SPHERO2_ADDR = "68:86:E7:02:8D:CF" SPHERO3_NAME = "Sphero-RGR" SPHERO3_ADDR = "68:86:E7:06:E7:1F" SPHERO4_NAME = "Sphero-OGG" SPHERO4_ADDR = "68:86:E7:02:8B:A0" sphero1 = sphero_driver.Sphero(SPHERO1_NAME, SPHERO1_ADDR) sphero2 = sphero_driver.Sphero(SPHERO2_NAME, SPHERO2_ADDR) sphero3 = sphero_driver.Sphero(SPHERO1_NAME, SPHERO1_ADDR) sphero4 = sphero_driver.Sphero(SPHERO2_NAME, SPHERO2_ADDR) sphero1.connect() sphero2.connect() sphero3.connect() sphero4.connect() sphero1.set_raw_data_strm(40, 1, 0, False) sphero2.set_raw_data_strm(40, 1, 0, False) sphero3.set_raw_data_strm(40, 1, 0, False) sphero4.set_raw_data_strm(40, 1, 0, False) sphero1.start() sphero2.start() sphero3.start()