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()
