def __init__(self): self.imu = ttypes.IMU(False, 0, 0, 0) self.accelerometer = ttypes.Accelerometer(False, 0, 0, 0) self.color = ttypes.Color(False, 0, 0, 0, 0, 0) self.ambient = ttypes.AmbientLight(False, 0) self.rvr = SpheroRvrObserver() self.rvr.wake() time.sleep(2) self.rvr.reset_yaw() self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.imu, handler=self.imu_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.color_detection, handler=self.color_detected_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.accelerometer, handler=self.accelerometer_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.ambient_light, handler=self.ambient_light_handler) self.rvr.sensor_control.start(interval=250)
def test_leds(self): """ This program demonstrates how to set the all the LEDs. """ rvr = SpheroRvrObserver() rvr.wake() # Give RVR time to wake up time.sleep(2) rvr.set_all_leds(led_group=RvrLedGroups.all_lights.value, led_brightness_values=[ color for _ in range(10) for color in Colors.off.value ]) # Delay to show LEDs change time.sleep(1) rvr.set_all_leds(led_group=RvrLedGroups.all_lights.value, led_brightness_values=[ color for _ in range(10) for color in [255, 0, 0] ]) # Delay to show LEDs change time.sleep(1) rvr.close()
def shutdown_rvr(self): """ This program will shut down RVR. """ rvr = SpheroRvrObserver() time.sleep(1) rvr.close()
def righty(self): rvr = SpheroRvrObserver() rvr.reset_yaw() rvr.drive_control.turn_right_degrees( heading=0, # Valid heading values are 0-359 amount=90)
def wake_rvr(self): """ This program will wake up RVR. """ rvr = SpheroRvrObserver() rvr.wake() # Give RVR time to wake up time.sleep(2)
def drive_backward(self): rvr = SpheroRvrObserver() rvr.reset_yaw() rvr.drive_control.drive_backward_seconds( speed=16, heading=0, # Valid heading values are 0-359 time_to_drive=1) rvr.close()
def drive_forward_rally(self): """ This program will move RVR forward fast. """ rvr = SpheroRvrObserver() rvr.reset_yaw() rvr.drive_control.drive_forward_seconds( speed=128, heading=0, # Valid heading values are 0-359 time_to_drive=1) rvr.close()
def drive_forward_crawl(self): """ This program will move RVR forwards slow. """ rvr = SpheroRvrObserver() rvr.reset_yaw() rvr.drive_control.drive_forward_seconds( speed=64, heading=0, # Valid heading values are 0-359 time_to_drive=.5) rvr.close()
def drive_forward_lightspeed(self): """ This program will move RVR forward at lightspeed. """ rvr = SpheroRvrObserver() rvr.reset_yaw() rvr.drive_control.drive_forward_seconds( speed=255, heading=0, # Valid heading values are 0-359 time_to_drive=.2) rvr.close()
def led_show_b(self): """ This program demonstrates how to set multiple LEDs on RVR using the LED control helper. """ rvr = SpheroRvrObserver() rvr.reset_yaw() try: rvr.wake() # Give RVR time to wake up time.sleep(2) rvr.led_control.turn_leds_off() # Delay to show LEDs change time.sleep(1) rvr.led_control.set_multiple_leds_with_enums( leds=[ RvrLedGroups.headlight_left, RvrLedGroups.headlight_right ], colors=[Colors.green, Colors.blue]) # Delay to show LEDs change time.sleep(1) rvr.led_control.set_multiple_leds_with_rgb( leds=[ RvrLedGroups.headlight_left, RvrLedGroups.headlight_right ], colors=[255, 0, 0, 0, 255, 0]) # Delay to show LEDs change time.sleep(1) except KeyboardInterrupt: print('\nProgram terminated with keyboard interrupt.') finally: rvr.close()
import os import sys import time sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__), '../../../'))) from sphero_sdk import SpheroRvrObserver from sphero_sdk import SpheroRvrTargets from sphero_sdk import ApiResponseCodesEnum from sphero_sdk.common.log_level import LogLevel rvr = SpheroRvrObserver(log_level=LogLevel.Debug_Verbose) def main(): """ This sample uses the generate_api_error command to intentionally generate an error response from RVR with the specified response code. Under normal circumstances certain commands request a response from RVR. If an error response is detected then it can be handled by the SDK's response logic. However, if a command does not have expected output, and therefore doesn't request a response (e.g. drive_with_heading), then RVR does not generate an error response if one occurs. Setting the request_error_responses_only flag on RVR will enable it to generate error responses even if no output is expected, and give a program the ability to catch any of the following 10 error response codes: Response Code 0x00: success Response Code 0x01: bad_did Response Code 0x02: bad_cid Response Code 0x03: not_yet_implemented Response Code 0x04: restricted Response Code 0x05: bad_data_length Response Code 0x06: failed Response Code 0x07: bad_data_value
def main(): connected = False # The callback for when the client receives a CONNACK response from the server. def on_connect(client, userdata, flags, rc): nonlocal connected connected = rc == 0 print("Connected with result code " + str(rc)) client = mqtt.Client() client.on_connect = on_connect client.connect("10.24.95.233", 8883, 60) rvr = SpheroRvrObserver() rvr.wake() time.sleep(2) rvr.reset_yaw() def imu_handler(imu_data): data = imu_data["IMU"] if connected: client.publish("sphero/imu", json.dumps(data)) def color_detected_handler(color_detected_data): data = color_detected_data["ColorDetection"] if connected: client.publish("sphero/color", json.dumps(data)) def accelerometer_handler(accelerometer_data): data = accelerometer_data["Accelerometer"] if connected: client.publish("sphero/accelerometer", json.dumps(data)) def ambient_light_handler(ambient_light_data): data = ambient_light_data["AmbientLight"] if connected: client.publish("sphero/ambient_light", json.dumps(data)) rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.imu, handler=imu_handler) rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.color_detection, handler=color_detected_handler) rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.accelerometer, handler=accelerometer_handler) rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.ambient_light, handler=ambient_light_handler) rvr.sensor_control.start(interval=250) try: client.loop_forever() except KeyboardInterrupt: print("Keyboard interrupt..") finally: client.disconnect() client.loop_stop()
class SpheroHandler: def __init__(self): self.imu = ttypes.IMU(False, 0, 0, 0) self.accelerometer = ttypes.Accelerometer(False, 0, 0, 0) self.color = ttypes.Color(False, 0, 0, 0, 0, 0) self.ambient = ttypes.AmbientLight(False, 0) self.rvr = SpheroRvrObserver() self.rvr.wake() time.sleep(2) self.rvr.reset_yaw() self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.imu, handler=self.imu_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.color_detection, handler=self.color_detected_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.accelerometer, handler=self.accelerometer_handler) self.rvr.sensor_control.add_sensor_data_handler( service=RvrStreamingServices.ambient_light, handler=self.ambient_light_handler) self.rvr.sensor_control.start(interval=250) def sense(self): return ttypes.SensorData(self.imu, self.accelerometer, self.ambient, self.color) def drive_with_heading(self, speed, heading, flags): #print(f"speed={speed}, heading={heading}, flags={flags}") self.rvr.drive_with_heading( speed=speed, # Valid speed values are 0-255 heading=heading, # Valid heading values are 0-359 flags=flags) time.sleep(0.1) def imu_handler(self, imu_data): data = imu_data["IMU"] self.imu.is_valid = data["is_valid"] self.imu.roll = data["Roll"] self.imu.pitch = data["Pitch"] self.imu.yaw = data["Yaw"] def color_detected_handler(self, color_detected_data): data = color_detected_data["ColorDetection"] self.color.is_valid = data["is_valid"] self.color.r = data["R"] self.color.g = data["G"] self.color.b = data["B"] self.color.index = data["Index"] self.color.confidence = data["Confidence"] def accelerometer_handler(self, accelerometer_data): data = accelerometer_data["Accelerometer"] self.accelerometer.is_valid = data["is_valid"] self.accelerometer.x = data["X"] self.accelerometer.Y = data["Y"] self.accelerometer.Z = data["Z"] def ambient_light_handler(self, ambient_light_data): data = ambient_light_data["AmbientLight"] self.ambient.is_valid = data["is_valid"] self.ambient.value = data["Light"]
def will_sleep_handler(): print('RVR is about to sleep...') rvr.wake() if __name__ == '__main__': global rvr try: rospy.init_node('rvr_controller') stay_awake = rospy.get_param('rvr_control/stay_awake') rvr = SpheroRvrObserver() rvr.wake() rospy.sleep(2) if stay_awake: rvr.on_will_sleep_notify(will_sleep_handler) sphero_sensor_pub = SpheroSensorPublisher(rvr) drive_control = DriveControlServer(rvr) rospy.spin() except rospy.ROSInterruptException: pass finally: rvr.close()
break self.rvr.raw_motors(left_mode=left_mode, left_speed=abs(left_speed), right_mode=right_mode, right_speed=abs(right_speed)) rospy.sleep(1) if left_speed == 0 and right_speed == 0: break if __name__ == '__main__': try: rospy.init_node('drive_control_server') rvr = SpheroRvrObserver() rvr.wake() # Give RVR time to wake up rospy.sleep(2) rvr.reset_yaw() server = DriveControlServer(rvr) rospy.spin() except rospy.ROSInterruptException: pass finally: rvr.close()
from subprocess import call # for getting the hostname and IP of the underlying system import socket # For Gamepad / joystick import struct, array from fcntl import ioctl #------------------- # RVR stuff #------------------- # create the RVR object. # This also lets the robot do a firmware check every now and then. print("Starting RVR observer...") rvr = SpheroRvrObserver() # RVR battery voltage handler def battery_percentage_handler(battery_percentage): print("The battery has {0:2d} % left.".format( battery_percentage["percentage"])) # store globally global batteryPercent batteryPercent = battery_percentage["percentage"] # RVR battery state handler #def battery_voltage_state_change_handler(battery_voltage_state): # #print("The battery voltage state is {0:1d}.".format(battery_voltage_state["state"])) # global batteryState
class MyServer(BaseHTTPRequestHandler): """ A special implementation of BaseHTTPRequestHander for reading data from and control GPIO of a Raspberry Pi """ rvr = SpheroRvrObserver() def do_HEAD(self): """ do_HEAD() can be tested use curl command 'curl -I http://server-ip-address:port' """ self.send_response(200) self.send_header('Content-type', 'text/html') self.end_headers() def _redirect(self, path): self.send_response(303) self.send_header('Content-type', 'text/html') self.send_header('Location', path) self.end_headers() def do_GET(self): """ do_GET() can be tested using curl command 'curl http://server-ip-address:port' """ self.do_HEAD() with open('control.html', 'rb') as file: self.wfile.write(file.read()) def do_POST(self): # Post data as dict post_data = json.loads( self.rfile.read(int( self.headers['Content-Length'])).decode("utf-8")) print("~~~~~~~~~~~~~~~~~~~~~") print(post_data) if post_data.get('system') == "travel": if post_data.get('type') == "on": print("on ") if post_data.get('command') == "left": # turn left print("left") self.rvr.raw_motors( left_mode=RawMotorModesEnum.reverse.value, left_duty_cycle=128, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.forward.value, right_duty_cycle=128 # Valid duty cycle range is 0-255 ) elif post_data.get('command') == "forward": # go forward print("forward") self.rvr.raw_motors( left_mode=RawMotorModesEnum.forward.value, left_duty_cycle=64, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.forward.value, right_duty_cycle=64 # Valid duty cycle range is 0-255 ) elif post_data.get('command') == "right": # turn right print("right") self.rvr.raw_motors( left_mode=RawMotorModesEnum.forward.value, left_duty_cycle=128, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.reverse.value, right_duty_cycle=128 # Valid duty cycle range is 0-255 ) elif post_data.get('command') == "backward": # go backward print("backward") self.rvr.raw_motors( left_mode=RawMotorModesEnum.reverse.value, left_duty_cycle=64, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.reverse.value, right_duty_cycle=64 # Valid duty cycle range is 0-255 ) if post_data.get('type') == "off": print("off") self.rvr.raw_motors( left_mode=RawMotorModesEnum.off.value, left_duty_cycle=0, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.off.value, right_duty_cycle=0 # Valid duty cycle range is 0-255 ) """ if post_data.get('command') == "left": print("left"); elif post_data.get('command') == "forward": print("forward"); self.rvr.raw_motors( left_mode=RawMotorModesEnum.off.value, left_duty_cycle=0, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.off.value, right_duty_cycle=0 # Valid duty cycle range is 0-255 ) elif post_data.get('command') == "right": print("right"); elif post_data.get('command') == "backward": print("backward"); self.rvr.raw_motors( left_mode=RawMotorModesEnum.off.value, left_duty_cycle=0, # Valid duty cycle range is 0-255 right_mode=RawMotorModesEnum.off.value, right_duty_cycle=0 # Valid duty cycle range is 0-255 ) """ elif post_data.get('system') == 'piLED': GPIO.output(28, not GPIO.input(28)) elif post_data.get('system') == 'RVRSetup': if post_data.get('command') == "on": # setup RVR print("Waking RVR") self.rvr.wake() time.sleep(2) self.rvr.reset_yaw() elif post_data.get('command') == "off": print("shutdown RVR") self.rvr.close() """ if post_data == 'On': GPIO.output(28, GPIO.HIGH) ser.write('on'.encode()) else: GPIO.output(28, GPIO.LOW) ser.write('off'.encode()) """ self.do_HEAD() self.wfile.write("received".encode("utf-8"))
import os import sys import time sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__), '../../../'))) from sphero_sdk import SpheroRvrObserver rvr = SpheroRvrObserver() # Handler for the active controller stopped notification. # After sending a stop command, your program can wait for # this async to confirm that the robot has come to a stop def stopped_handler(): print('RVR has stopped') def main(): """ This program has RVR drive around using the normalized RC drive command. """ try: rvr.wake() # Give RVR time to wake up time.sleep(2) # Register the handler for the stopped notification rvr.on_robot_has_stopped_notify(handler=stopped_handler)
# RVR stuff #------------------- import os import sys sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__), './lib/'))) from sphero_sdk import SpheroRvrObserver from sphero_sdk import Colors from sphero_sdk import RvrLedGroups from sphero_sdk import DriveFlagsBitmask # create the RVR object. # This also lets the robot do a firmware check every now and then. print("Starting RVR observer...") rvr = SpheroRvrObserver() def turnLEDs(R, G, B): rvr.led_control.set_all_leds_rgb(R, G, B) # Delay to show LEDs change sleep(1) #------------------------- # Wake up, Mr. Freeman! #------------------------- print("Waking up RVR...") rvr.wake() # Give RVR time to wake up sleep(2)