From 845f61e710cdf171ff7a02e460167d2d3001fa76 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Wed, 15 Apr 2026 18:12:23 +0100 Subject: [PATCH] Initial commit Nothing has been tested yet. --- .devcontainer/Dockerfile | 56 ++ .devcontainer/devcontainer.json | 42 ++ .vscode/settings.json | 5 + my_robot/__init__.py | 0 my_robot/motor_controller_node.py | 118 +++++ package.xml | 17 + raspbot_v2_interface/README.md | 5 + .../Raspbot_Lib.egg-info/PKG-INFO | 6 + .../Raspbot_Lib.egg-info/SOURCES.txt | 8 + .../Raspbot_Lib.egg-info/dependency_links.txt | 1 + .../Raspbot_Lib.egg-info/top_level.txt | 1 + .../Raspbot_Lib-checkpoint.py | 480 ++++++++++++++++++ .../Raspbot_Lib/Raspbot_Lib.py | 480 ++++++++++++++++++ raspbot_v2_interface/Raspbot_Lib/__init__.py | 1 + .../__pycache__/Raspblock.cpython-311.pyc | Bin 0 -> 6187 bytes .../__pycache__/Raspblock_Lib.cpython-311.pyc | Bin 0 -> 6183 bytes .../__pycache__/Raspbot_Lib.cpython-311.pyc | Bin 0 -> 17648 bytes .../__pycache__/__init__.cpython-311.pyc | Bin 0 -> 235 bytes raspbot_v2_interface/__init__.py | 3 + .../build/lib/Raspbot_Lib/Raspbot_Lib.py | 480 ++++++++++++++++++ .../build/lib/Raspbot_Lib/__init__.py | 1 + .../dist/Raspblock_Lib-0.0.2-py3.11.egg | Bin 0 -> 5444 bytes .../dist/Raspbot_Lib-0.0.2-py3.11.egg | Bin 0 -> 10939 bytes raspbot_v2_interface/motor_controller.py | 109 ++++ raspbot_v2_interface/setup.py | 11 + setup.cfg | 0 setup.py | 16 + 27 files changed, 1840 insertions(+) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100644 .vscode/settings.json create mode 100644 my_robot/__init__.py create mode 100644 my_robot/motor_controller_node.py create mode 100644 package.xml create mode 100644 raspbot_v2_interface/README.md create mode 100644 raspbot_v2_interface/Raspbot_Lib.egg-info/PKG-INFO create mode 100644 raspbot_v2_interface/Raspbot_Lib.egg-info/SOURCES.txt create mode 100644 raspbot_v2_interface/Raspbot_Lib.egg-info/dependency_links.txt create mode 100644 raspbot_v2_interface/Raspbot_Lib.egg-info/top_level.txt create mode 100644 raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py create mode 100644 raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py create mode 100644 raspbot_v2_interface/Raspbot_Lib/__init__.py create mode 100644 raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc create mode 100644 raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc create mode 100644 raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc create mode 100644 raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc create mode 100644 raspbot_v2_interface/__init__.py create mode 100644 raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py create mode 100644 raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py create mode 100644 raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg create mode 100644 raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg create mode 100644 raspbot_v2_interface/motor_controller.py create mode 100644 raspbot_v2_interface/setup.py create mode 100644 setup.cfg create mode 100644 setup.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..44ad8e4 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,56 @@ +FROM ros:kilted +ARG USERNAME=USERNAME +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Delete user if it exists in container (e.g Ubuntu Noble: ubuntu) +RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi + +# Create the user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + # + # [Optional] Add sudo support. Omit if you don't need to install software after connecting. + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME +RUN apt-get update && apt-get upgrade -y + +# C++ development tools +RUN apt-get install -y \ + build-essential \ + cmake \ + gdb \ + clang \ + clang-format \ + clang-tidy \ + libboost-all-dev + +# Python development tools +RUN apt-get install -y \ + python3-pip \ + python3-dev \ + python3-argcomplete \ + python3-colcon-common-extensions \ + python3-colcon-mixin \ + python3-rosdep \ + python3-vcstool + +# ROS 2 ament build tools +# RUN apt-get install -y \ +# ros-${ROS_DISTRO}-ament-cmake \ +# ros-${ROS_DISTRO}-ament-cmake-auto \ +# ros-${ROS_DISTRO}-ament-python \ +# ros-${ROS_DISTRO}-ament-lint-auto \ +# ros-${ROS_DISTRO}-ament-lint-common + +ENV SHELL /bin/bash + +# ******************************************************** +# * Anything else you want to do like clean up goes here * +# ******************************************************** + +# [Optional] Set the default user. Omit if you want to keep the default as root. +USER $USERNAME +CMD ["/bin/bash"] \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..255c54a --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,42 @@ +{ + "name": "ROS 2 Development Container", + "privileged": true, + "remoteUser": "matt", + "build": { + "dockerfile": "Dockerfile", + "args": { + "USERNAME": "matt" + } + }, + "workspaceFolder": "/home/ws", + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ws,type=bind", + "customizations": { + "vscode": { + "extensions":[ + "ms-vscode.cpptools", + "ms-vscode.cpptools-themes", + "twxs.cmake", + "donjayamanne.python-extension-pack", + "eamodio.gitlens", + "ms-iot.vscode-ros", + "anthropic.claude-code" + ] + } + }, + "containerEnv": { + "DISPLAY": "unix:0", + "ROS_AUTOMATIC_DISCOVERY_RANGE": "LOCALHOST", + "ROS_DOMAIN_ID": "42" + }, + "runArgs": [ + "--net=host", + "--pid=host", + "--ipc=host", + "-e", "DISPLAY=${env:DISPLAY}" + ], + "mounts": [ + "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", + "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached" + ], + "postCreateCommand": "sudo rosdep update && sudo rosdep install --from-paths src --ignore-src -y && sudo chown -R $(whoami) /home/ws/" +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c578b33 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "python.analysis.extraPaths": [ + "/opt/ros/kilted/lib/python3.12/site-packages" + ] +} diff --git a/my_robot/__init__.py b/my_robot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/my_robot/motor_controller_node.py b/my_robot/motor_controller_node.py new file mode 100644 index 0000000..7d07d8a --- /dev/null +++ b/my_robot/motor_controller_node.py @@ -0,0 +1,118 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import Float32MultiArray + +# Import your motor library +from raspbot_v2_interface import MotorController + +class MotorControllerNode(Node): + def __init__(self): + super().__init__('motor_controller') + + # --- Parameters --- + self.declare_parameter('wheel_base', 0.3) # meters between left/right wheels + self.declare_parameter('max_speed', 1.0) # max motor speed (library units) + + self.wheel_base = self.get_parameter('wheel_base').value + self.max_speed = self.get_parameter('max_speed').value + + # --- Initialize your motor library --- + self.motors = MotorController() + self.motors.initialize() + + # --- Subscribers --- + # cmd_vel is the standard ROS2 topic for velocity commands + self.cmd_vel_sub = self.create_subscription( + Twist, + 'cmd_vel', + self.cmd_vel_callback, + 10 # QoS queue depth + ) + + # Optional: direct per-wheel speed control [FL, FR, RL, RR] + self.wheel_speeds_sub = self.create_subscription( + Float32MultiArray, + 'wheel_speeds', + self.wheel_speeds_callback, + 10 + ) + + # --- Publishers --- + # Publish current wheel speeds for feedback + self.speed_pub = self.create_publisher(Float32MultiArray, 'current_wheel_speeds', 10) + + # --- Timer for telemetry --- + self.telemetry_timer = self.create_timer(0.1, self.publish_telemetry) # 10 Hz + + self.get_logger().info('Motor controller node started') + + def cmd_vel_callback(self, msg: Twist): + """ + Convert a Twist message (linear.x, angular.z) into + differential-drive wheel speeds for a 4-wheeled robot. + """ + linear = msg.linear.x # m/s forward + angular = msg.angular.z # rad/s rotation + + # Differential drive kinematics + left_speed = linear - (angular * self.wheel_base / 2.0) + right_speed = linear + (angular * self.wheel_base / 2.0) + + # Clamp to max speed + left_speed = max(-self.max_speed, min(self.max_speed, left_speed)) + right_speed = max(-self.max_speed, min(self.max_speed, right_speed)) + + # Send to motors via your library (FL=FR=left, RL=RR=right) + try: + self.motors.set_speed(front_left=left_speed, front_right=right_speed, + rear_left=left_speed, rear_right=right_speed) + except Exception as e: + self.get_logger().error(f'Motor error: {e}') + + def wheel_speeds_callback(self, msg: Float32MultiArray): + """Direct per-wheel control: [FL, FR, RL, RR]""" + if len(msg.data) != 4: + self.get_logger().warn('wheel_speeds expects exactly 4 values [FL, FR, RL, RR]') + return + try: + self.motors.set_speed(front_left=msg.data[0], front_right=msg.data[1], + rear_left=msg.data[2], rear_right=msg.data[3]) + except Exception as e: + self.get_logger().error(f'Motor error: {e}') + + def publish_telemetry(self): + """Read and publish current wheel speeds from your library.""" + try: + speeds = self.motors.get_speeds() # Expected: [FL, FR, RL, RR] + msg = Float32MultiArray() + msg.data = [float(s) for s in speeds] + self.speed_pub.publish(msg) + except Exception as e: + self.get_logger().warn(f'Telemetry read failed: {e}') + + def destroy_node(self): + """Clean shutdown — stop motors before exiting.""" + self.get_logger().info('Shutting down motors...') + try: + self.motors.stop_all() + self.motors.shutdown() + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = MotorControllerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..96e4b03 --- /dev/null +++ b/package.xml @@ -0,0 +1,17 @@ + + + my_robot + 0.0.1 + Four-wheel robot motor controller + Your Name + Apache-2.0 + + rclpy + geometry_msgs + std_msgs + + ament_python + ament_copyright + ament_flake8 + ament_pep257 + \ No newline at end of file diff --git a/raspbot_v2_interface/README.md b/raspbot_v2_interface/README.md new file mode 100644 index 0000000..52a7df0 --- /dev/null +++ b/raspbot_v2_interface/README.md @@ -0,0 +1,5 @@ +## Installation Steps (安装步骤) + +cd py_install + +sudo python3 setup.py install \ No newline at end of file diff --git a/raspbot_v2_interface/Raspbot_Lib.egg-info/PKG-INFO b/raspbot_v2_interface/Raspbot_Lib.egg-info/PKG-INFO new file mode 100644 index 0000000..4715f50 --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib.egg-info/PKG-INFO @@ -0,0 +1,6 @@ +Metadata-Version: 2.1 +Name: Raspbot-Lib +Version: 0.0.2 +Summary: Raspbot drvier V0.0.2 +Home-page: www.yahboom.com +Author: Yahboom Team diff --git a/raspbot_v2_interface/Raspbot_Lib.egg-info/SOURCES.txt b/raspbot_v2_interface/Raspbot_Lib.egg-info/SOURCES.txt new file mode 100644 index 0000000..5fee8f9 --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib.egg-info/SOURCES.txt @@ -0,0 +1,8 @@ +README.md +setup.py +Raspbot_Lib/Raspbot_Lib.py +Raspbot_Lib/__init__.py +Raspbot_Lib.egg-info/PKG-INFO +Raspbot_Lib.egg-info/SOURCES.txt +Raspbot_Lib.egg-info/dependency_links.txt +Raspbot_Lib.egg-info/top_level.txt \ No newline at end of file diff --git a/raspbot_v2_interface/Raspbot_Lib.egg-info/dependency_links.txt b/raspbot_v2_interface/Raspbot_Lib.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/raspbot_v2_interface/Raspbot_Lib.egg-info/top_level.txt b/raspbot_v2_interface/Raspbot_Lib.egg-info/top_level.txt new file mode 100644 index 0000000..810b2e0 --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib.egg-info/top_level.txt @@ -0,0 +1 @@ +Raspbot_Lib diff --git a/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py b/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py new file mode 100644 index 0000000..d568cab --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py @@ -0,0 +1,480 @@ +#!/usr/bin/env python3 +# coding: utf-8 +import smbus +import time,random +import math + +PI5Car_I2CADDR = 0x2B +class Raspbot(): + + def get_i2c_device(self, address, i2c_bus): + self._addr = address + if i2c_bus is None: + return smbus.SMBus(1) + else: + return smbus.SMBus(i2c_bus) + + def __init__(self): + # Create I2C device. + self._device = self.get_i2c_device(PI5Car_I2CADDR, 0) + + #写数据 + def write_u8(self, reg, data): + try: + self._device.write_byte_data(self._addr, reg, data) + except: + print ('write_u8 I2C error') + + def write_reg(self, reg): + try: + self._device.write_byte(self._addr, reg) + except: + print ('write_u8 I2C error') + + def write_array(self, reg, data): + try: + # self._device.write_block_data(self._addr, reg, data) + self._device.write_i2c_block_data(self._addr, reg, data) + except: + print ('write_array I2C error') + + #读数据 + def read_data_byte(self): + try: + buf = self._device.write_byte(self._addr) + return buf + except: + print ('read_u8 I2C error') + + def read_data_array(self,reg,len): + try: + buf = self._device.read_i2c_block_data(self._addr,reg,len) + return buf + except: + print ('read_u8 I2C error') + + +#控制电机 + def Ctrl_Car(self, motor_id, motor_dir,motor_speed): + try: + if(motor_dir !=1)and(motor_dir != 0): #参数非法,方向默认前进 + motor_dir = 0 + if(motor_speed>255): + motor_speed = 255 + elif(motor_speed<0): + motor_speed = 0 + + reg = 0x01 + data = [motor_id, motor_dir, motor_speed] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制电机正反(-255~255) + def Ctrl_Muto(self, motor_id, motor_speed): + try: + + if(motor_speed>255): + motor_speed = 255 + if(motor_speed<-255): + motor_speed = -255 + if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 + motor_dir = 1 + else:motor_dir = 0 + reg = 0x01 + data = [motor_id, motor_dir, abs(motor_speed)] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制舵机 + def Ctrl_Servo(self, id, angle): + try: + reg = 0x02 + data = [id, angle] + if angle < 0: + angle = 0 + elif angle > 180: + angle = 180 + if(id==2 and angle > 100):angle = 100 + self.write_array(reg, data) + except: + print ('Ctrl_Servo I2C error') + +#控制灯珠(全部) + def Ctrl_WQ2812_ALL(self, state, color): + try: + reg = 0x03 + data = [state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独控制灯珠 + def Ctrl_WQ2812_Alone(self, number,state, color): + try: + reg = 0x04 + data = [number,state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制亮度(全部) + def Ctrl_WQ2812_brightness_ALL(self, R, G, B): + try: + reg = 0x08 + data = [R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独灯珠亮度 + def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): + try: + reg = 0x09 + data = [number,R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制红外遥控器开关 + def Ctrl_IR_Switch(self, state): + try: + reg = 0x05 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_IR_Switch I2C error') + +#控制蜂鸣器开关 + def Ctrl_BEEP_Switch(self, state): + try: + reg = 0x06 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_BEEP_Switch I2C error') + +#控制超声波测距开关 + def Ctrl_Ulatist_Switch(self, state): + try: + reg = 0x07 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_getDis_Switch I2C error') + + + + +#控制灯珠特效 +class LightShow: + + def __init__(self): + self.num_lights = 14 + self.last_val = 0 + self.MAX_TIME=999999 + self.bot=Raspbot() + self.running = True + + def execute_effect(self, effect_name,effect_duration,speed,current_color): + try: + if effect_name == 'river': + self.run_river_light(effect_duration,speed) + elif effect_name == 'breathing': + self.breathing_light(effect_duration,speed,current_color) + elif effect_name == 'gradient': + self.gradient_light(effect_duration,speed) + elif effect_name == 'random_running': + self.random_running_light(effect_duration,speed) + elif effect_name == 'starlight': + self.starlight_shimmer(effect_duration,speed) + else: + print("Unknown effect name.") + except KeyboardInterrupt: + self.turn_off_all_lights() + self.running = False + + def turn_off_all_lights(self): + self.bot.Ctrl_WQ2812_ALL(0, 0) + + def run_river_light(self,effect_duration,speed): + # speed = 0.01 + colors = [0, 1, 2, 3, 4, 5, 6] + color_index = 0 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights - 2): + self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) + time.sleep(speed) + self.bot.Ctrl_WQ2812_ALL(0, 0) + time.sleep(speed) + color_index = (color_index + 1) % len(colors) + self.turn_off_all_lights() + + def breathing_light(self, effect_duration,speed,current_color): + breath_direction = 0 + breath_count = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + + if current_color == 0: # Red + r, g, b = breath_count, 0, 0 + elif current_color == 1: # Green + r, g, b = 0, breath_count, 0 + elif current_color == 2: # Blue + r, g, b = 0, 0, breath_count + elif current_color == 3: # Yellow + r, g, b = breath_count, breath_count, 0 + elif current_color == 4: # Purple + r, g, b = breath_count, 0, breath_count + elif current_color == 5: # Cyan + r, g, b = 0, breath_count, breath_count + elif current_color == 6: # White + r, g, b = breath_count, breath_count, breath_count + else: + r, g, b = 0, 0, 0 # Default to black if invalid color code + + self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) + time.sleep(speed) + + if breath_direction == 0: + breath_count += 2 + if breath_count >= 255: + breath_count=255 + breath_direction = 1 + else: + breath_count -= 2 + if breath_count < 0: + breath_direction = 0 + breath_count = 0 + self.turn_off_all_lights() + + + def random_running_light(self,effect_duration,speed): + # on_time = 0.01 + # off_time = 0.01 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights): + color = random.randint(0, 6) + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + self.turn_off_all_lights() + + def starlight_shimmer(self,effect_duration,speed): + min_lights_on = 1 + max_lights_on = 7 + colors = [0, 1, 2, 3, 4, 5, 6] + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for color in colors: + start_time = time.time() + while time.time() - start_time < 1: + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) + for i in lights_on: + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + self.turn_off_all_lights() + + def gradient_light(self, effect_duration, speed): + grad_color = 0 + grad_index = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + if grad_color % 2 == 0: + gt_red = random.randint(0, 255) + gt_green = random.randint(0, 255) + gt_blue = random.randint(0, 255) + + gt_green = self.rgb_remix(gt_green) + gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) + grad_color += 1 + + if grad_color == 1: + if grad_index < 14: + number = (grad_index % 14) + 1 # Adjusting for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 2 + grad_index = 0 + + elif grad_color == 3: + if grad_index < 14: + number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 0 + grad_index = 0 + + time.sleep(speed) + + self.turn_off_all_lights() + + def rgb_remix(self, val): + if abs(val - self.last_val) < val % 30: + val = (val + self.last_val) % 255 + self.last_val = val % 255 + return self.last_val + + def rgb_remix_u8(self, r, g, b): + if r > 50 and g > 50 and b > 50: + index = random.randint(0, 2) + if index == 0: + r = 0 + elif index == 1: + g = 0 + elif index == 2: + b = 0 + return r, g, b + + def calculate_breath_color(self, color_code, breath_count): + max_brightness = 255 + if color_code == 0: # Red + return max_brightness, breath_count, 0 + elif color_code == 1: # Green + return max_brightness, 0, breath_count + elif color_code == 2: # Blue + return max_brightness, 0, 0, breath_count + elif color_code == 3: # Yellow + return max_brightness, breath_count, breath_count, 0 + elif color_code == 4: # Purple + return max_brightness, breath_count, 0, breath_count + elif color_code == 5: # Cyan + return max_brightness, 0, breath_count, breath_count + elif color_code == 6: # White + return max_brightness, breath_count, breath_count, breath_count + else: + return 0, 0, 0 # Default to black if invalid color code + + def stop(self): + self.running = False + +# test +#car = Raspbot() + +#读取巡线传感器地址 ,此值只有1位 +# track =car.read_data_array(0x0a,1) +# track = int(track[0]) +# x1 = (track>>3)&0x01 +# x2 = (track>>2)&0x01 +# x3 = (track>>1)&0x01 +# x4 = (track)&0x01 +# print(track,x1,x2,x3,x4) + + +# 读取超声传感器地址,此值只有2位 +# car.Ctrl_Ulatist_Switch(1)#open +# time.sleep(1) +# diss_H =car.read_data_array(0x1b,1)[0] +# diss_L =car.read_data_array(0x1a,1)[0] +# dis = diss_H<< 8 | diss_L +# print(dis+"mm") +# car.Ctrl_Ulatist_Switch(0)#close + +#读取红外遥控的值 +# car.Ctrl_IR_Switch(1) +# time.sleep(3) +# data =car.read_data_array(0x0c,1) +# print(data) +# car.Ctrl_IR_Switch(0) + + +#蜂鸣器测试 +# car.Ctrl_BEEP_Switch(1) +# time.sleep(1) +# car.Ctrl_BEEP_Switch(0) +# time.sleep(1) + + +#电机测试 +# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 +# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 +# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 +# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 +# time.sleep(1) +# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 +# time.sleep(1) +# car.Ctrl_Car(0,0,0) #L1电机 停止 +# car.Ctrl_Car(1,0,0) #L2电机 停止 +# car.Ctrl_Car(2,0,0) #R1电机 停止 +# car.Ctrl_Car(3,0,0) #R2电机 停止 + + +#舵机测试 +# car.Ctrl_Servo(1,0) #s1 0度 +# car.Ctrl_Servo(2,180) #s2 180度 +# time.sleep(1) +# car.Ctrl_Servo(1,180) #s1 180度 +# car.Ctrl_Servo(2,0) #s2 0度 +# time.sleep(1) + +#灯条测试 +# car.Ctrl_WQ2812_ALL(1,0)#红色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,1)#绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,2)#蓝色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,3)#黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#单个灯测试 +# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(14,1,2)#14号绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#控制亮度测试 全部 +# for i in range(255): +# car.Ctrl_WQ2812_brightness_ALL(i,0,0) +# time.sleep(0.01) + diff --git a/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py b/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py new file mode 100644 index 0000000..4b101ce --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py @@ -0,0 +1,480 @@ +#!/usr/bin/env python3 +# coding: utf-8 +import smbus +import time,random +import math + +PI5Car_I2CADDR = 0x2B +class Raspbot(): + + def get_i2c_device(self, address, i2c_bus): + self._addr = address + if i2c_bus is None: + return smbus.SMBus(1) + else: + return smbus.SMBus(i2c_bus) + + def __init__(self): + # Create I2C device. + self._device = self.get_i2c_device(PI5Car_I2CADDR, 1) + + #写数据 + def write_u8(self, reg, data): + try: + self._device.write_byte_data(self._addr, reg, data) + except: + print ('write_u8 I2C error') + + def write_reg(self, reg): + try: + self._device.write_byte(self._addr, reg) + except: + print ('write_u8 I2C error') + + def write_array(self, reg, data): + try: + # self._device.write_block_data(self._addr, reg, data) + self._device.write_i2c_block_data(self._addr, reg, data) + except: + print ('write_array I2C error') + + #读数据 + def read_data_byte(self): + try: + buf = self._device.write_byte(self._addr) + return buf + except: + print ('read_u8 I2C error') + + def read_data_array(self,reg,len): + try: + buf = self._device.read_i2c_block_data(self._addr,reg,len) + return buf + except: + print ('read_u8 I2C error') + + +#控制电机 + def Ctrl_Car(self, motor_id, motor_dir,motor_speed): + try: + if(motor_dir !=1)and(motor_dir != 0): #参数非法,方向默认前进 + motor_dir = 0 + if(motor_speed>255): + motor_speed = 255 + elif(motor_speed<0): + motor_speed = 0 + + reg = 0x01 + data = [motor_id, motor_dir, motor_speed] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制电机正反(-255~255) + def Ctrl_Muto(self, motor_id, motor_speed): + try: + + if(motor_speed>255): + motor_speed = 255 + if(motor_speed<-255): + motor_speed = -255 + if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 + motor_dir = 1 + else:motor_dir = 0 + reg = 0x01 + data = [motor_id, motor_dir, abs(motor_speed)] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制舵机 + def Ctrl_Servo(self, id, angle): + try: + reg = 0x02 + data = [id, angle] + if angle < 0: + angle = 0 + elif angle > 180: + angle = 180 + if(id==2 and angle > 110):angle = 110 + self.write_array(reg, data) + except: + print ('Ctrl_Servo I2C error') + +#控制灯珠(全部) + def Ctrl_WQ2812_ALL(self, state, color): + try: + reg = 0x03 + data = [state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独控制灯珠 + def Ctrl_WQ2812_Alone(self, number,state, color): + try: + reg = 0x04 + data = [number,state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制亮度(全部) + def Ctrl_WQ2812_brightness_ALL(self, R, G, B): + try: + reg = 0x08 + data = [R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独灯珠亮度 + def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): + try: + reg = 0x09 + data = [number,R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制红外遥控器开关 + def Ctrl_IR_Switch(self, state): + try: + reg = 0x05 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_IR_Switch I2C error') + +#控制蜂鸣器开关 + def Ctrl_BEEP_Switch(self, state): + try: + reg = 0x06 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_BEEP_Switch I2C error') + +#控制超声波测距开关 + def Ctrl_Ulatist_Switch(self, state): + try: + reg = 0x07 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_getDis_Switch I2C error') + + + + +#控制灯珠特效 +class LightShow: + + def __init__(self): + self.num_lights = 14 + self.last_val = 0 + self.MAX_TIME=999999 + self.bot=Raspbot() + self.running = True + + def execute_effect(self, effect_name,effect_duration,speed,current_color): + try: + if effect_name == 'river': + self.run_river_light(effect_duration,speed) + elif effect_name == 'breathing': + self.breathing_light(effect_duration,speed,current_color) + elif effect_name == 'gradient': + self.gradient_light(effect_duration,speed) + elif effect_name == 'random_running': + self.random_running_light(effect_duration,speed) + elif effect_name == 'starlight': + self.starlight_shimmer(effect_duration,speed) + else: + print("Unknown effect name.") + except KeyboardInterrupt: + self.turn_off_all_lights() + self.running = False + + def turn_off_all_lights(self): + self.bot.Ctrl_WQ2812_ALL(0, 0) + + def run_river_light(self,effect_duration,speed): + # speed = 0.01 + colors = [0, 1, 2, 3, 4, 5, 6] + color_index = 0 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights - 2): + self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) + time.sleep(speed) + self.bot.Ctrl_WQ2812_ALL(0, 0) + time.sleep(speed) + color_index = (color_index + 1) % len(colors) + self.turn_off_all_lights() + + def breathing_light(self, effect_duration,speed,current_color): + breath_direction = 0 + breath_count = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + + if current_color == 0: # Red + r, g, b = breath_count, 0, 0 + elif current_color == 1: # Green + r, g, b = 0, breath_count, 0 + elif current_color == 2: # Blue + r, g, b = 0, 0, breath_count + elif current_color == 3: # Yellow + r, g, b = breath_count, breath_count, 0 + elif current_color == 4: # Purple + r, g, b = breath_count, 0, breath_count + elif current_color == 5: # Cyan + r, g, b = 0, breath_count, breath_count + elif current_color == 6: # White + r, g, b = breath_count, breath_count, breath_count + else: + r, g, b = 0, 0, 0 # Default to black if invalid color code + + self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) + time.sleep(speed) + + if breath_direction == 0: + breath_count += 2 + if breath_count >= 255: + breath_count=255 + breath_direction = 1 + else: + breath_count -= 2 + if breath_count < 0: + breath_direction = 0 + breath_count = 0 + self.turn_off_all_lights() + + + def random_running_light(self,effect_duration,speed): + # on_time = 0.01 + # off_time = 0.01 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights): + color = random.randint(0, 6) + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + self.turn_off_all_lights() + + def starlight_shimmer(self,effect_duration,speed): + min_lights_on = 1 + max_lights_on = 7 + colors = [0, 1, 2, 3, 4, 5, 6] + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for color in colors: + start_time = time.time() + while time.time() - start_time < 1: + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) + for i in lights_on: + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + self.turn_off_all_lights() + + def gradient_light(self, effect_duration, speed): + grad_color = 0 + grad_index = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + if grad_color % 2 == 0: + gt_red = random.randint(0, 255) + gt_green = random.randint(0, 255) + gt_blue = random.randint(0, 255) + + gt_green = self.rgb_remix(gt_green) + gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) + grad_color += 1 + + if grad_color == 1: + if grad_index < 14: + number = (grad_index % 14) + 1 # Adjusting for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 2 + grad_index = 0 + + elif grad_color == 3: + if grad_index < 14: + number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 0 + grad_index = 0 + + time.sleep(speed) + + self.turn_off_all_lights() + + def rgb_remix(self, val): + if abs(val - self.last_val) < val % 30: + val = (val + self.last_val) % 255 + self.last_val = val % 255 + return self.last_val + + def rgb_remix_u8(self, r, g, b): + if r > 50 and g > 50 and b > 50: + index = random.randint(0, 2) + if index == 0: + r = 0 + elif index == 1: + g = 0 + elif index == 2: + b = 0 + return r, g, b + + def calculate_breath_color(self, color_code, breath_count): + max_brightness = 255 + if color_code == 0: # Red + return max_brightness, breath_count, 0 + elif color_code == 1: # Green + return max_brightness, 0, breath_count + elif color_code == 2: # Blue + return max_brightness, 0, 0, breath_count + elif color_code == 3: # Yellow + return max_brightness, breath_count, breath_count, 0 + elif color_code == 4: # Purple + return max_brightness, breath_count, 0, breath_count + elif color_code == 5: # Cyan + return max_brightness, 0, breath_count, breath_count + elif color_code == 6: # White + return max_brightness, breath_count, breath_count, breath_count + else: + return 0, 0, 0 # Default to black if invalid color code + + def stop(self): + self.running = False + +# test +#car = Raspbot() + +#读取巡线传感器地址 ,此值只有1位 +# track =car.read_data_array(0x0a,1) +# track = int(track[0]) +# x1 = (track>>3)&0x01 +# x2 = (track>>2)&0x01 +# x3 = (track>>1)&0x01 +# x4 = (track)&0x01 +# print(track,x1,x2,x3,x4) + + +# 读取超声传感器地址,此值只有2位 +# car.Ctrl_Ulatist_Switch(1)#open +# time.sleep(1) +# diss_H =car.read_data_array(0x1b,1)[0] +# diss_L =car.read_data_array(0x1a,1)[0] +# dis = diss_H<< 8 | diss_L +# print(dis+"mm") +# car.Ctrl_Ulatist_Switch(0)#close + +#读取红外遥控的值 +# car.Ctrl_IR_Switch(1) +# time.sleep(3) +# data =car.read_data_array(0x0c,1) +# print(data) +# car.Ctrl_IR_Switch(0) + + +#蜂鸣器测试 +# car.Ctrl_BEEP_Switch(1) +# time.sleep(1) +# car.Ctrl_BEEP_Switch(0) +# time.sleep(1) + + +#电机测试 +# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 +# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 +# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 +# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 +# time.sleep(1) +# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 +# time.sleep(1) +# car.Ctrl_Car(0,0,0) #L1电机 停止 +# car.Ctrl_Car(1,0,0) #L2电机 停止 +# car.Ctrl_Car(2,0,0) #R1电机 停止 +# car.Ctrl_Car(3,0,0) #R2电机 停止 + + +#舵机测试 +# car.Ctrl_Servo(1,0) #s1 0度 +# car.Ctrl_Servo(2,180) #s2 180度 +# time.sleep(1) +# car.Ctrl_Servo(1,180) #s1 180度 +# car.Ctrl_Servo(2,0) #s2 0度 +# time.sleep(1) + +#灯条测试 +# car.Ctrl_WQ2812_ALL(1,0)#红色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,1)#绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,2)#蓝色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,3)#黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#单个灯测试 +# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(10,1,2)#10号绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#控制亮度测试 全部 +# for i in range(255): +# car.Ctrl_WQ2812_brightness_ALL(i,0,0) +# time.sleep(0.01) + diff --git a/raspbot_v2_interface/Raspbot_Lib/__init__.py b/raspbot_v2_interface/Raspbot_Lib/__init__.py new file mode 100644 index 0000000..3e40211 --- /dev/null +++ b/raspbot_v2_interface/Raspbot_Lib/__init__.py @@ -0,0 +1 @@ +from .Raspbot_Lib import Raspbot,LightShow \ No newline at end of file diff --git a/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2b20dc499e7e9a8055f8f022b6c62777aa643c64 GIT binary patch literal 6187 zcmdT|+fN(W89y`Q0c;Kz+uUy@OHwd*oS;oNyQFN`Y?Efw-Joi_yo`)zLYx>(&X{De zjDn(lplMXOQKayQ+HDF-TBX%K@YwzbHnJ4U(n^u4R^rVSeTep@{k}8f@r(ma$x>-M z9{+sqKIixQzVDow-__K32we2;FRtdA3Hd8#ikECA_9h_lh^RyrqGZPS6=npU7iUCO zoRLKGAyK6-i0aZ@k0nB$z}L8DWL20Z0eKg*mv;vM!$ysPf|=Qyg~TJG5gZr>6}})d zk}3kaa9|k51z4fVfR$+`cqdS0IX==~)fie0)Ey_&DYI!)K}m62drfd)EGq`SNO zoBok&v3YG|F*35atV9;{cqkeju~t_~4KFU|Yv;7Mf&-}9tw>l)v{>zioi)`kxIPH5 zN^*@IYp(mAT(xiQ?AQL5hiBGr{%YdkM4BG^8_(U&_CN1Ydv6IqSH|XaI9(4Lj)4Xb(&YX zc?sk*-tKehGjdx16H54Qvq)T1mQHsA72O(RF&1#K2I!x!Gpc5npwC#TR^5tF?40rd~3G!R-52Ucg6BJsObPF-h^j{3*ItW^^{oUgtAts42Yag)(&YMXWwlapWA2+EGPUW>$3{c^7J18IGOC( zp*BfWo7={ zESD3_+K@3b0ve`TZ5uDqSWYtkn;z^`G+38=@P%QpLpyY6Q+;}UyJtI|p;K8pl`h_# z=s|9<{TVutr2}c(4F=1)V9fnPUL0x;Z>Y}wVMR#7zIpg?;AL7!bwmT!4bw60GcV09 zU3J)JEpLzhuV9?8?+eZv+`08B0JKz>yD#T!dN}yp7ufLyHjh8;&-li(zVWqlIe*jI zMNVEHC-3_i+MA`gM_4zIXIg0w`HlDO|FM;>NJ;6iM6zC_%=C9Z=iwlGgjTrUaL3O} zQEh?sAyF8#V2d_`KwXJSZjOo8l9iTK1+jNAAe3kh?)ZYwPJQ*xcKFXd8Q=M=@BHDY ze#;E3IIrS`X}*R10TSF^;n;NpO9-nrK(@}nn2cuj`hf9d)hy~HU%5&bK0>(JJzQ+U`us)((AVZJj&gDcao65 z5btDz)6XpNQ_u*-2l*tFZ%zamQG6%|?m*-f5yTcP`5^`7aKL#kDJ5M;BJVCLYsmW* zq!JCB=V^`Iij@Rrqfix)4P`~pR1Pi7MKzm*cucb?=qZhoXejw$sr1LtUS9-272WOu zh;pb%)4H5G19H?gq;B3@UR%z!w5H^gobxuV`yaHY+H)=Kkb9k213V%IGPFNS`_r}? z&p&uJyE`;S{^*^Q4D;AdKXWfTxdN!qiML#iTIDqvadB9&D0F6D2Xe*@SH2oL{)2Oy7>g!`I4G zgeMq&R^jTEghOM%7-^iQYn?;b4}{Jd2Ho@p07I~mqQ|A){?=NJaIJnRn9lf29>3Cu~}^aXR1s;De9%SBrg!1Utp4FYl9RD|0E^cz2uvjUAytV_cw- z3?0qV(X{Obf!GE3-~FH9zneaUuUl-oba0O0rPx$N|L>LY^IkuVhT;)DZgxv_mWku6 zzVFoM zm^A^iBbYS-vSXMv0kS7CTMh;VxbQ#)!D4K?n z5^dj9y=y2m!9Ilwe7~q`00l`9ghB-oYE~(Ja-@2df2_m{Z{uoh!S#_Sz>N0CRK-7d L%|8#Q=hOH%I5uD) literal 0 HcmV?d00001 diff --git a/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dc728c10967891629b196953b3d3e93ff6ed51cb GIT binary patch literal 6183 zcmdT|+fN(W89y`Q0c;Kz+uUy@OHwd*J3*UnvZQR-Y?HF-Zcs1F%gA^p#EH4&j7b*D zC@9(onnslyMGB9o-KL50%>f2R5=4%hF1bs#fBSiatd9(th8W@p#68revwK z9S=Xwea`v)zVAC{<~LPU9s<{&{`j-2+l_?$6*Gm)WhVB{LgEooi7Ld%l<{Af5_n#m z5>;_Z63GWdl|Cb?>#<146ZkW(DVHitl7PH}*~>e9fDxm{K*7xHO+(@l(FhI;Lkgdf zDM=LpT{!HNthxZpR2i^bEd#7j%K_bL1)xWD16HaYK&n;(R;d)QTCD=CQL6!K)f$*c zZQgw)tS?N*<|8*El5HlK#=kw4Pb4WMiDFeJ#g)3at@U+leX8&nmhhyo3^GafZR{KF`O(i@; z?*~{Rx%#$M*L_c}(zkljj5W=cefnZh+{R0xKg@5_#k1md!4>RRr==|9D_?3LQ zxv+r1DAemXmrhoLO)C^2DH>Olgs-^hLYWU6w*#z@7mcmY8+*4Kdw)B+70fign{9j- zmQPpV$Gp(OVKy=p@b!HPiATf@E4&UV<65^fQ7FU~)_TCq#K+M8368-p0o-enbP<8X zah>K>YGwlYgtz-t`h?sTz?2fc+bkN_l*QAXKt-p<*gOlkSRM4w*BDjPOHi+d<6-8; zQh9+z=i-|Z=P55SZ6@!++JbqRq!os>lU68$ctP|_04royl-z+Ft-U|-oc3(fo{hSv zEzc&mj%Vn(EIpSl+%I=U!5#RLdei>y4DHF%p0w?L`SRsn1q^rh*a`A$?{}n##S;`- zmUIg;Z^E?Gs@{ZWh6~;_S@jfI<%F_ks0@glPSy%(9cSNZ8lT%}4J;@8MeDN)(em`~ z0XUiL+NNFWbg8>vH$MFbsBJn+|NKPp@xvZN)Qm zJWI#Zg_{%I&keRWL;JF{FKxTQU|9!@xqrxuL(Sn0)wxeB3rW~N4<8P^Obe@yXuvvQ zI;MT*rRl}14*RU;?a}`gj1%^K!C8YlxIPYmmg;i%Rs zvU)z}Z&jv^nEA1k`^1l5)w$f!ODIJzb){T^z{_f{I9AuBs z3iliC_<1R&&9NRN3WF9HsA-`!MJl;DCYp;@T2d9l-i3frq&c+h3q3pa#XDP(KX+w( z7qY$!hokx}GqB>k3Kyo)k@kKE!0i=|UpM%M#43_-x--*|S{9R%D&63{8Kjn_xe6!| zZ}71gwvbd|*@fF$Qks#L<%2f-q$?>O7?An1`H*;?giW+~zH)AS0r@h+K*uoD?)(cN z;X5B^F$InU#gMWovJ=>d2k0_mzpI#?kIyqDs^;BBN{zC7rIFMZG)?77vNq_amjR%N z1sN*}kFf8Fg|ai)#(IdJn8jY z0UqV_YiZrarsWTu)OL0`36Je58kn|1KvJ<4pTG56qCXXkCt!jday|z$tJji@y#Y= z6X*Qm6=m{vG#G+pf+!hRrh)-dR%&jA>M4HY~t_4e1+ zYKUt!kfDQFI+(WIR-o)3CeOTMuKz2o@Hk=9s)f_}-uv%=w7**H10>ifPHkzQL`#WF z*~Yu$6exIv0tLB1Lm4`prNe344Fa)?@ZA01;J=wZgr8e%y0meQ;iK4iRR8al@$+6k zjD_P-J#KbOw3mqEtiJEm=j0LUbCT;5%+OGlhSIjXpwH!i7r%?mg=aNI$$J!Kc3xeK zVVWw+&BbucsNvt*SUnPiUn1St|y%hCd_@G=SIq>+8A#jIc zDVCn4&Pc!X8JavB&KStA@r;o*oG}@wqZDiWfMQLb8Yt>B_>-XKg?otQ{!BI)GNQ4A2_38mN=40qSC%K;5hhXf5jo>S1f4X&$la)nMe} zsA(C7@$WH98WyMHR6tFR8?Jeyn%w7APO$W+(526mZc(SH^7-P1(tP&}Cm&)lzfm*? zf-K94=E&USLPRv5e(B&s1form(Ck^U3e!T7h*&i+_%7qnMrMU@ZwdmW;zrNU?ptm^2}5yRxjrr)W@R+MFxOeqY8B` z3DXUG!BLwSTlO^G-jlj?bN}uAJk$AW@^nS;Y|MK$^32A^ke`}T*#HlyZiJynAaI9@ zQ)&2Jdqq`NQlOA}WdaeXsVuOTtAg@=BjmU~m1MxJ~P0>^odnsR~ z>P1s;l6>zP-=wb4phB_QE8I*p6j<2V0gUJfaojxTH*sDlFFK{9$*Yjg2BSgFhOy?0 z+)Oxn&q%l=8oAK4Xu?!Qtz?!d)fQQ%#MDDAk>fzFQL6@{&0k<#H%>fY`f^NP#(Tdx zdwQuyV8-&y7_Zzbxzs$IK&qjgi%QJnv=6>?z$zv;2_kQOF>Tlhy=Bd5NQ^HQJ84~>l!c49O!VzqJhpc!uDL3#q=2kRMjUu^U zfZTdMcITMx^u#B}vqpj0nrF81%Kc0(MhF)>dZK% zQVGt@&QDz^YAs`k7&oj>8Q4{6OI5LP1uC z16(IGRMt(=IJt0Er<*k-KaTtWB1CaE(8u>911Er>mYQsRh1&Yt0}pEbxmtgw=YGFX zyDeY4Epe#esZSgytm`AJ>lK)dc?M?&^#<0-8rn_$(7ogLtf7m>xbb<}$n_z^X&pq#RHI*Y6%${1@AV75rJoONzKI?n}WSkWeh*cncANWsX(P>3aRU0_=Qr#$e z_7vcN*1W%WoT7d~xyQW~#@H_*lPY>iyr2XiZWKSHA&ZIl4IqB$*+$aP>Y+dtOrQ#o z2I4mn@f)KWi>K7ol@&vigH%b#Qx$`}P0GufpQ;#^liU`_I75H+)mKD2Bt5}l1$MMC zI2l=^6Wk~ya@&w>MS`<&c?GZ$alf<>oi9$}Y7|Z_k&l3&3RK(tD>`AL=(pya?Yy(S z(AEW_hv!qM3h6!Bo?Q2KzI!`S5m1uWwqz4vtE>y3dophc{#|+hF5a{Ichw05s`tko z<4)=qPWO1XUMW!g1P|^*0F?q1!H6Sz+5n<0Dg=!CMFWg~R0&08jG!_!pa(J72zr>* zG!vv%F!|%gc*%NFW0q;r;8{X{h%c7V;HMCJ7zPsa64{&%ahKSt%BOcxVIp8~`BGS#tF$b8-)e%jr#Cx_&irwb0m< zG$+jkcYVrp%a`;O8h!BObSHzLYOBl@2{~)Ndlws$XFG^GrXlxp7)W z!~Xp}W7N;x^xUlf=*3Mf$x@4wZ^eo{KHvg}F&sr%qt`!$>MauX|0Uu_oPMloTZ}j_<=2Vk81YlH-Uh z$L7?w^akG3_dAPngp9x~0yCUvhI!4cj=;eY>L>1TO9fG;p=o7AiDM9XDb2Y6yt-#J z!L;ediX(Epf;B#MCN-UWNK7o8PPs!!2y)7@qhA&2qauCqDI#muifodbnLZZ{gZGMz zz^-z#)93jvyfX3~ASgtKIvDGc!4#d`fiiR@yRTnPT!sZllE$ae>sJz23f>0kNlL4& zgQsWNQO_Ql+jdd^Dc7`POVOIunV?#bQ34X!MCc(@SUc8{zKvn(gKhe3VHugQ+TMV=zG_ZFchzW|j$*9mYqQz~d-BmJ*3y$fYUxQ!mAX^7xM+ z>i4mC3cI55mKz!Mp$Gh+vx)N6i=XouB9U=~{#5v{X97E(Ykf?X8R_*1WvK9C%%(oc`?0YujzNZtG zc<@tQsK=-3Wq0GvrrQ`}Ej0MxN3%7kwj7FMm~iSoa0HD9jgO?UqOn)OIDoPS*}u{R zWmO)-JJbukui#OEcb0A^lI6VQoxwMgkXlR*VCiyZ36wR-Z^p9Nqczu4%9=H0RViyL zs8lxUDCUCG1<>Q-?z&LXgGxga|e3P;5nE*NI#=K``~i8gRVa6}hl-fx93gy*k> zJ3?pAhNhw&2&aQb{5H`EnFFLwNoz%?TC*Hcqtq`yd-ZRTW8Lbz0+Dkwb91nOMhCa( zelc`)az4nh$HGyFTeuh%J<$a&9GE|QHUQ@Vq~Z~77BVaTFNf4fG8>3a`GH;FU?!Un z!zl!Gh>O)z3mgZ%4iGm>1eF@iMa5nWy&IZZfPH1DbgZ#Bob)MqC>Ln~A)>|Tv~{8f zbUNqUz&ke-T;2z+o}8;E9msx5aP7&v_VCILKJv69=ibD-H|er1?Gs$b@~&gNa$`n& z&fU+u`$@*`oU1$ia`v#`+MRdp=9L?=@f}-o?k&7~3(4otx%`=-r454XK;CtLSMJjH z_U2r@>G!e=f@@#iwU1ZsU%Tq~`T@bUIq%xcEBBLSX9rAIw$36UYtWt?1AhG8Jk!fF zy#-%KV&cY$1o^jWhTP~qK-77^^&&^<{1waJOJf5Rb<@!x7`pgL&A z;Y9#vF9h(U#d-LZuJVAvg44F#kcS;wg0`?{h)W!4ZZCz;tB?|FEy}x$w*5C`iF5z~ zB(*X(tPiXmIcrDSogNpgy?JXduiR_TRp1~defb!`>JBvx?&yzU)dk*3lXj1x*hBpQ z$1SMyDo7@2u4z4`;^vs00+_~5L$|+*YTz8|Q=yMFVm>`2O;GRr7}osMc@^Z+n$n~~ zLgE&f;sB!9Ypd15;W8}+$)f>kW`#}{C3e4odFO>ScW+VHX}mZIm}1K-HRkPG8+nAl*S<9 z1BV08FS~8NjS+FoW|8n&bdX;-=fsBI6|JE#8^9Nc^h^ZBL7-a(Zw`H=v8Lh(E}5bp zLUQC7f$SA`FHi1-rBL6Ha4dToZtuFe=k}h2wNO)gWA+hcwe9}v=-sOye*5mXGj9m( z+m@WU_M?3J(d9bW`MNunIVJc8g}R}99azAY>MlaSH|O%+wmxw6=Un}n;Y=`FzvRdd zPYABVdDmgyby%N#>fUIU&Te1Ylpj4JxQ^ysM|s!Lb!1tFm)KL=1XpL?)yca$_n~#Y_GA!wM+-3 zSc7_X*SSnJyW#fw$YQX33&(n{cECEK(rZjIYYK~~2tRU%~o4dfxW z!Ua7t#;W1X*f-k+0`HuOwJn-)S_(T!dV`#xBGaz{NIGd{9{?bgol?pIl3kj@ zr48^(a0I9Skb4?}{(irOdmm#Flq4V`OZpG+k${g~@+7L@!!1IR=#nhj0Gz1+(-%!h zMvj@9UkFFJKf_EkC(_d*JsE)sR6!%fn&hGorv;=E6ZwB2Ir8@ekP5yQ-r87bXiP8~ zELuSCG_NV1*Pqv$t#tzNdT&);6n@S!wRPSp|)R#^Ry17alM}Ic5lua5x(jn4NiM zC(rC$hLq#U1%YYJGp#(+y5ek459gdcytAj^f?fmoC7FV!Jw2N9^zxqGLT69%L}6ne z@9lZAqRY^k^K|o`ZYizM4cS)5WWcM$*VO~`(-BWKoIrqRJP^Y!34**s>Bb=>Nta%u z;T(c?GQYE)GI3&}sixdwB~wmsI!sXSjDmrwnfA}CBMz{x#Bd=-CiaJ1<=35Huo&UY z6ZBZSCcm*&*9UbSS_Xf#Jzt+0qw9Hg@wvP|LSA+>^vydyo1R?;JKynyJ=?N?DGDODOH1>#YnJukgJ3(tAUqHrd)Nd`6 zw^3(?fLGAgR(9yiW{6O?DPIRZJwx?QLXz}yW82-D^rcLv&^VB99Dtqt5|gA(aP7#u zcJSmb9cHUGd1zF6@2Y+MBLlU7jEwC-;kyiZxdTfdBP@NshD`v=o}f<8;04m9A76T_ z07@N+neAx?qAkDBHPF;ug=ls=fJp)g{8roan>G% z`C7Tos|%9#y<1ndI7Zi;TDo6yrg708w*!)_x7~ESs!Y38%Z{SXt9Y~)AD*A{+5aQX zC+QnqI^6cHtF1blCrM|I0DhsZu;$Y1u)GX|1Ar(f4nJ+;{uBw?H-1{AFZdnhkW2$y zHzB8Ji3H~^!htZ{oz_5^qr~D7tLJ9IvQI4lmQM9t@ZI7g_dP64&<=r4w4>85N)}=S z{zTg>@mxd6hyyZ!0i;~g?#LC0jQNU4xW?%>fH3$2AmCX6=XAA1imNn@Q6w+W9JZZn zkx^(rT4-vAt%vHJkB}tI%dMSv-~8~cyKg1!%k|B-XKr4&eIb2Zs2|L_bM+&9{m61_ zS9=(@dxr07Cp%U$b(#*tj(2%08KZlQ5czHtvv?&Xf&PpUr;WhZ|6 zvM@N2ADsA=<(D=5>nwjZBD@gIzYyg+7KDz4e8)oaSfRBSt2{tUbZyObLgQGzaf~PT zGRy(TQwxH3L*BcA_ip%YHPzrN)9&BaP|fYB=-u~(rlEWjtU_rw%mF}>;{~YVg$Lfj zoOdvDDchMnC3tt_y*qevKQdy*IvM9HadEYA{Wy;n1HpNF5$9ozu%6Me#;x`8dP?aX z8-VXyVQpnVH;KBoiE0s1B^wAU7az+?SGVTyRJTAFCfu?HCe$vqFTzCu?W`ArquZNzms<7oL#)L zYq_cYZX2v)M_ac6Pxtk|!%}U+U7W_GEjp?)J{~@ekk2b_s1;f2X>CU9vD<-6bP= zB^IyleL9k>-X@SUj(URR=#tj2W#J&8X63eay8!os5pQJtsV>P6^+^bgUx7&>*soN_ zqqc6?Avd0UNRDX`=Z%5e38lFiAnG??us5##=8O9)NQ`-k_JulbSk%u6NWDfaJ8BY< zNS^g%}<)zypz`9(n_(LXEs{%KbTJ!ya&d2}Cp##3^8yn2Dz}H|rvrU(KZ?3ftup+nUj7e_a~6T1(8f5sYin6z{&hgwycPom^h0#Tqkij;}S$8 zV~+a_A3sNeq9;9)7F9UlI3)iGf06$Ha!vh*N6uRC*qBUeTxQt5S4j_T-Uy2Yg;;9L z-FQw+CLJ3vq>@)?8$PR~-i?nc>Fvfxl^mx{=2kq;CKJYHZotAafx255): + motor_speed = 255 + elif(motor_speed<0): + motor_speed = 0 + + reg = 0x01 + data = [motor_id, motor_dir, motor_speed] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制电机正反(-255~255) + def Ctrl_Muto(self, motor_id, motor_speed): + try: + + if(motor_speed>255): + motor_speed = 255 + if(motor_speed<-255): + motor_speed = -255 + if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 + motor_dir = 1 + else:motor_dir = 0 + reg = 0x01 + data = [motor_id, motor_dir, abs(motor_speed)] + self.write_array(reg, data) + except: + print ('Ctrl_Car I2C error') + +#控制舵机 + def Ctrl_Servo(self, id, angle): + try: + reg = 0x02 + data = [id, angle] + if angle < 0: + angle = 0 + elif angle > 180: + angle = 180 + if(id==2 and angle > 110):angle = 110 + self.write_array(reg, data) + except: + print ('Ctrl_Servo I2C error') + +#控制灯珠(全部) + def Ctrl_WQ2812_ALL(self, state, color): + try: + reg = 0x03 + data = [state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独控制灯珠 + def Ctrl_WQ2812_Alone(self, number,state, color): + try: + reg = 0x04 + data = [number,state, color] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制亮度(全部) + def Ctrl_WQ2812_brightness_ALL(self, R, G, B): + try: + reg = 0x08 + data = [R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812 I2C error') + +#单独灯珠亮度 + def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): + try: + reg = 0x09 + data = [number,R,G,B] + if R >255: + R =255 + if G > 255: + G = 255 + if B >255: + B=255 + self.write_array(reg, data) + except: + print ('Ctrl_WQ2812_Alone I2C error') + +#控制红外遥控器开关 + def Ctrl_IR_Switch(self, state): + try: + reg = 0x05 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_IR_Switch I2C error') + +#控制蜂鸣器开关 + def Ctrl_BEEP_Switch(self, state): + try: + reg = 0x06 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_BEEP_Switch I2C error') + +#控制超声波测距开关 + def Ctrl_Ulatist_Switch(self, state): + try: + reg = 0x07 + data = [state] + if state < 0: + state = 0 + elif state > 1: + state = 1 + self.write_array(reg, data) + except: + print ('Ctrl_getDis_Switch I2C error') + + + + +#控制灯珠特效 +class LightShow: + + def __init__(self): + self.num_lights = 14 + self.last_val = 0 + self.MAX_TIME=999999 + self.bot=Raspbot() + self.running = True + + def execute_effect(self, effect_name,effect_duration,speed,current_color): + try: + if effect_name == 'river': + self.run_river_light(effect_duration,speed) + elif effect_name == 'breathing': + self.breathing_light(effect_duration,speed,current_color) + elif effect_name == 'gradient': + self.gradient_light(effect_duration,speed) + elif effect_name == 'random_running': + self.random_running_light(effect_duration,speed) + elif effect_name == 'starlight': + self.starlight_shimmer(effect_duration,speed) + else: + print("Unknown effect name.") + except KeyboardInterrupt: + self.turn_off_all_lights() + self.running = False + + def turn_off_all_lights(self): + self.bot.Ctrl_WQ2812_ALL(0, 0) + + def run_river_light(self,effect_duration,speed): + # speed = 0.01 + colors = [0, 1, 2, 3, 4, 5, 6] + color_index = 0 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights - 2): + self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) + self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) + time.sleep(speed) + self.bot.Ctrl_WQ2812_ALL(0, 0) + time.sleep(speed) + color_index = (color_index + 1) % len(colors) + self.turn_off_all_lights() + + def breathing_light(self, effect_duration,speed,current_color): + breath_direction = 0 + breath_count = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + + if current_color == 0: # Red + r, g, b = breath_count, 0, 0 + elif current_color == 1: # Green + r, g, b = 0, breath_count, 0 + elif current_color == 2: # Blue + r, g, b = 0, 0, breath_count + elif current_color == 3: # Yellow + r, g, b = breath_count, breath_count, 0 + elif current_color == 4: # Purple + r, g, b = breath_count, 0, breath_count + elif current_color == 5: # Cyan + r, g, b = 0, breath_count, breath_count + elif current_color == 6: # White + r, g, b = breath_count, breath_count, breath_count + else: + r, g, b = 0, 0, 0 # Default to black if invalid color code + + self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) + time.sleep(speed) + + if breath_direction == 0: + breath_count += 2 + if breath_count >= 255: + breath_count=255 + breath_direction = 1 + else: + breath_count -= 2 + if breath_count < 0: + breath_direction = 0 + breath_count = 0 + self.turn_off_all_lights() + + + def random_running_light(self,effect_duration,speed): + # on_time = 0.01 + # off_time = 0.01 + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for i in range(self.num_lights): + color = random.randint(0, 6) + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + self.turn_off_all_lights() + + def starlight_shimmer(self,effect_duration,speed): + min_lights_on = 1 + max_lights_on = 7 + colors = [0, 1, 2, 3, 4, 5, 6] + end_time = time.time() + while self.running and time.time() - end_time < effect_duration: + for color in colors: + start_time = time.time() + while time.time() - start_time < 1: + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) + for i in lights_on: + self.bot.Ctrl_WQ2812_Alone(i, 1, color) + time.sleep(speed) + for i in range(self.num_lights): + self.bot.Ctrl_WQ2812_Alone(i, 0, 0) + self.turn_off_all_lights() + + def gradient_light(self, effect_duration, speed): + grad_color = 0 + grad_index = 0 + end_time = time.time() + + while self.running and time.time() - end_time < effect_duration: + if grad_color % 2 == 0: + gt_red = random.randint(0, 255) + gt_green = random.randint(0, 255) + gt_blue = random.randint(0, 255) + + gt_green = self.rgb_remix(gt_green) + gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) + grad_color += 1 + + if grad_color == 1: + if grad_index < 14: + number = (grad_index % 14) + 1 # Adjusting for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 2 + grad_index = 0 + + elif grad_color == 3: + if grad_index < 14: + number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing + self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) + grad_index += 1 + if grad_index >= 14: + grad_color = 0 + grad_index = 0 + + time.sleep(speed) + + self.turn_off_all_lights() + + def rgb_remix(self, val): + if abs(val - self.last_val) < val % 30: + val = (val + self.last_val) % 255 + self.last_val = val % 255 + return self.last_val + + def rgb_remix_u8(self, r, g, b): + if r > 50 and g > 50 and b > 50: + index = random.randint(0, 2) + if index == 0: + r = 0 + elif index == 1: + g = 0 + elif index == 2: + b = 0 + return r, g, b + + def calculate_breath_color(self, color_code, breath_count): + max_brightness = 255 + if color_code == 0: # Red + return max_brightness, breath_count, 0 + elif color_code == 1: # Green + return max_brightness, 0, breath_count + elif color_code == 2: # Blue + return max_brightness, 0, 0, breath_count + elif color_code == 3: # Yellow + return max_brightness, breath_count, breath_count, 0 + elif color_code == 4: # Purple + return max_brightness, breath_count, 0, breath_count + elif color_code == 5: # Cyan + return max_brightness, 0, breath_count, breath_count + elif color_code == 6: # White + return max_brightness, breath_count, breath_count, breath_count + else: + return 0, 0, 0 # Default to black if invalid color code + + def stop(self): + self.running = False + +# test +#car = Raspbot() + +#读取巡线传感器地址 ,此值只有1位 +# track =car.read_data_array(0x0a,1) +# track = int(track[0]) +# x1 = (track>>3)&0x01 +# x2 = (track>>2)&0x01 +# x3 = (track>>1)&0x01 +# x4 = (track)&0x01 +# print(track,x1,x2,x3,x4) + + +# 读取超声传感器地址,此值只有2位 +# car.Ctrl_Ulatist_Switch(1)#open +# time.sleep(1) +# diss_H =car.read_data_array(0x1b,1)[0] +# diss_L =car.read_data_array(0x1a,1)[0] +# dis = diss_H<< 8 | diss_L +# print(dis+"mm") +# car.Ctrl_Ulatist_Switch(0)#close + +#读取红外遥控的值 +# car.Ctrl_IR_Switch(1) +# time.sleep(3) +# data =car.read_data_array(0x0c,1) +# print(data) +# car.Ctrl_IR_Switch(0) + + +#蜂鸣器测试 +# car.Ctrl_BEEP_Switch(1) +# time.sleep(1) +# car.Ctrl_BEEP_Switch(0) +# time.sleep(1) + + +#电机测试 +# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 +# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 +# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 +# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 +# time.sleep(1) +# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 +# time.sleep(1) +# car.Ctrl_Car(0,0,0) #L1电机 停止 +# car.Ctrl_Car(1,0,0) #L2电机 停止 +# car.Ctrl_Car(2,0,0) #R1电机 停止 +# car.Ctrl_Car(3,0,0) #R2电机 停止 + + +#舵机测试 +# car.Ctrl_Servo(1,0) #s1 0度 +# car.Ctrl_Servo(2,180) #s2 180度 +# time.sleep(1) +# car.Ctrl_Servo(1,180) #s1 180度 +# car.Ctrl_Servo(2,0) #s2 0度 +# time.sleep(1) + +#灯条测试 +# car.Ctrl_WQ2812_ALL(1,0)#红色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,1)#绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,2)#蓝色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(1,3)#黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#单个灯测试 +# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 +# time.sleep(1) +# car.Ctrl_WQ2812_Alone(10,1,2)#10号绿色 +# time.sleep(1) +# car.Ctrl_WQ2812_ALL(0,0) #关闭 + +#控制亮度测试 全部 +# for i in range(255): +# car.Ctrl_WQ2812_brightness_ALL(i,0,0) +# time.sleep(0.01) + diff --git a/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py b/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py new file mode 100644 index 0000000..3e40211 --- /dev/null +++ b/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py @@ -0,0 +1 @@ +from .Raspbot_Lib import Raspbot,LightShow \ No newline at end of file diff --git a/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg b/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg new file mode 100644 index 0000000000000000000000000000000000000000..f28ecfd8e80c1f5b452d7e101bc56c020cab6548 GIT binary patch literal 5444 zcma)Aby!qe`yEm`1SFMikQAk)bI5_A1{fNK7`l;CKq-j<}_08{2WMvDkUkq7!J8Xe^5 zP*PFhSJP6~7SI9yd7?RhB(e|~_E^G!`@#CO1!)}j=0v`0g$`G_ z4CCcX5n@E2ue0Z7~Wykc~E%K^k?RJaoF5Ed>I;s;2 z>EL`2LAj_hxZS+_cA8WPPQ5F!35PpW=NE#@=MmQ4Xkt_7_(}V3f_mCukb;ulT`xZ` zLJx1Reea2W%_SZ!es2C@-Z!fR>U?)qKlEu2#h>aCkF?B{gb4>`%E4mc^>w% zGwv|bt?tmhH3aE~1J%?WEsEd9;Md-V z<>Rr)0r0u?Mc@2}70tJ|%Vy;FTZ%McMjcxO4_<^$P3|zqERdWBJH-Pg-<{n)wA*TN z_kHZ`nW?z+hh2$P+ah0rJm2qS(gqrQ_O0KPh~ka(rZGvAwZ=88Eb|D-29)>=Ru-wq zQfS_$+uak{BA$_JD>F=odqu@c#KN~E8iyAq7kp2ShF|PDyq-W>b82aX=F7s1d;4T3 z#?7cTHsJYI`AHAtodM8$4{(Z@M~P}mCKlQA34nJ+U$f;*I+A#q;U|zw3E*S2D@oys zE{ECUF|6oX^Tc9A627nEM@1y{hrQV}iXCce5b4c;tC3q%J2aXpK7s8kiVjv16~GXf z*_D7EhBvHum4cxo460+6b{bFTi$vA7cre7%4Sx@=UlC|MqK0<)oB9x8*Q~oS6=J<+ z*BMJ3di^BxC8gTKI1F6fm+hCTHr%P1y(@}NU`wn*GWvK+W*rH#b%jlZUSt*){@mvW zk#Q9Bx+R-@moq=SdPnR7lJ!vo)(&0Ed*aS7-4 zusgo0_Q&Oyj>ZSB=PPqRknh}^&X?I)+?PwP6@4p+s&#jt*sGhFxH;NZ?0E$g=dy)4lSuy2jCd+9;T1c<>hAF}Kh}BZa7zf`x2g`w@ z*jGF|VKYrT4(|a6T(Mdu$Ew=LNZlMFYNh27vqXd=qK@FpC$Y5MwB}N6$pslIz2)H3 z8i}`Wrjh8C!IFB(P#(Y4Iiq&05Vhu<=wDDKF*d@GHDy#g_}O7?mux<{jgpCvG+ zKV$N8wQwl$bC1Dk>EQ3UCM0;&wK0pEG)4|*qOeaZH|<-bt}nnJX)q`VhQ zkMFb`L8XtNP}f$o!SOSCJ(b89pSy=?fu`%Qr&=`d_~G^uY^DvCAI1}XO`xM>(%YTA zYOb59rzh>}-=d!H?R_`gP4t2KZ1rUllk%=Kvt?k4YUm-&lr<|jlD06F zrAEIPoNDq|YV6G>L0<`maffgzDV1&|{cxT}oCJfLCLTSza*JI(y!;`R8gZ8DMHe;i zVZ*G1x4M+-9-{9Bz8J$Sv|^7s#v@TF*Kzvps_MbVh(TzDg$aJ>St1!FZ;_7O)Cf39 zA(syEKsE86Ge;xCtGN6}h_^>IYfmIwW8i8klK!?DN<&LkY`_l*e3+J| zmhqvtV=!u~SU;rFu*d5*#p&k1cvk*}yH`l*aCN&Aj|UXZC~XmD&h$GJ(-Phtav`Xf zR=_hA?6`OzNo+2D1YALKn4q|MC0V&e$HAeQsi|?LHy?(Ok)|*x)=n^E6NgjYbs%&D zWMHXFc)$mtj}j6kd@-5768bIGR-A(~jahJA6*K4=17dWQa?i&cU4?9QH=Ip3*&|bK z%8W?1N|AmfU|f)91+8w&KF5DSH6p*CI)9RPBd#0F(UA1n{D62prPJse%jl!U97Qk~ zSM5Vf%Hy<6ihnwn5_k^iW`im{pGli_PuyNNcvRmk{skOZ8&JY z^=VZKOSU67u-H6(69z)MS&l;QMZH+GwPT2Q0_KV>J0XjebZvi8SFh-r%S4k()Hf?f zkvT9QhJmW22p^!}OU4Ob^Pp`fXvoY_DQ7a`4#W#sQxDJEVK-ur!7g3n34hp-ZV|V` zx{>CDS~&x*eX-t}=%eKy62B8Ea`CF~$WmAXSIxjWvc;@aJOEm|&Lsj(gh>g$4(}OR zKNsHl7R>D9Ocl*s&Za74+`}%H^(qq#>^O_!vk zTwDZ5w1+s7JT40|b!y(*u-psu4B?AhNh2XS;nmYS@8K&P=_(Le@I5vt4p1LqqtP-b z=Ui5A9CFz6oDA%X8#l+S({(rOm%4=6j?ll5**B|Px)_OYYQ6nlCX?dyA~_Yl`Zz1X z!d`192zM+-L=1mi9Wgy7QR6`Fax!WpsqNrx{OqZvUU_nqvHG2p#OI)675hl7?n*BX z4;~3&kN5iU0j4es34FgyF;ikQUN-JTb%!KGxsbifC+wVUxWvNi<3a7MCc~|!);qxr zLz9dGI=E(=}=L;x~5_(&l@7al_pNk{dgfU#A6Ob?BiC4x6p z<;#ldkaaE~I|;8I`yCU_=d_5IozsYy_l{+!jMb;;`u*jVkn3Y3oK}<3pKJ8LS8k4+ zt;9F`S!!|m{6K#OZ@(*-2LwAs@viN9|8lix7hZeq zr~?VCw1*P$25Fwm1l5fBC{|=ti*t1D*f9TY&Cb#E38VRj@jRb<@;hp?`L{*qVU}BV zfeLPuowv&4jj(;Fu)I$Lv$5rQ!-he++d2V=5u$i;5( zPLa5%K+i}YeI&_R5uSBY`PL-+!m@{H&5~Cj$=m>yR$q|RktIv1jW5Q1GNr#oQnr8+ za(T+s3$jRNFveHBLmYfKzjC%PeHbVt+kOqSRs zj{tkjQUUyCyv$KI5KAv$=D444w^E;HY!E6Woj+*7&739`pry5#WcabfkV>gYV%aVs zcOYSS8h;S-Su)qylk(eTPl6^JZ+(R&bC54Q7>;!3KJnkSazfhre$qR-$Inpzeg8*q z_oo(P*H4h6I=;-NI?1M{&yAehkLcd8=6$N=V@plQlrADnoOR(%U!Wz`0v;xSPX#BjhP<69iG#u zchj*g&mdr#NZw#qqnr;E0b4ie3m8rJf9v~tLJ2wDT=N`m^gGh+RsY^@`d!YE+$w3V zY^o=3Eu!KvuoV>*PbshB(SABVPQl{GvK9Ajxb9&{^_&ZVlQfF1h)H>oDeturOPLEE z2~lE6PO2b(CwYuB)xK$D2rqf7`dR_`C2^P+e_^m``=mXC6=0L=orP4w3ra-3)17xlcuT4Z=E1cP{ zLVU*5*@2_b5H_JuL$KDp8~!=VlkEXspls>6s=zTfR!nlv3E@@RMb?x0I~_aYt9 zv(YB5EEySUafW#3TttqekR~_&{!2F46o@a|nvGjR!qGKShw_o+zJ7kOWmm>eh(eNDd=d268$r)l1(9S`irBoIkx_27$YrRr$g zc1%v|w|rYj5YG8PYSN&fp6CxB2A?iGR~6-Uerv44;{kuz4`ulnsLAW4DtG6&Mq5ut zNa>uy=LMqKULD0w#O ze{g;(Gi}NnH!1gQ$Y&<2ZT4 z!ia+hg$}Iv$03vKsuka0!bR-*gNULj_PC~fQn)usvywN7j(3N@5QvObTt#?-mJnx= zb41_SB(VTg$fo%b^-c{HljP+hmO2jE`1rQ|Rt}DZ06<#V7o+WYmmk?8S(|&P9)JP#q^rtFr;40Rz)5K)Gf}wpbUQkHm@ti2 z=N+8OS4amZ&3M>=Se)<#Od#!q3NKQp8OH?0$_@cQJ5iY!1vHW9KRTD(1$gZUy? z-ox?MZJ%ShOWsuG(*=f0rTqm+^=Q-?+IzWb(I|{)kLRIf)C@WV{|`C!w^+NTr65E7 z{oMfU>@;mvJ;3O5+5`Xv;(`yGtw^Lmut0EKa8+1YNLaW)5J$v8xYY(*-9dQXiu6yN zw?Agn*iCHP``w0vN(gJsP&f#e_Ml(1SX6{lNrs#KUA>|Jr$|M${Cy!Lmw4xZqRc&M zGe)(H;GiHob33)5W^=hU0g{Z_ABfRR{U#QY9BwAU7~8O}8+OXdJ@iBb)Rnm`VmSn8 z`G|o@iS@rtDfFcO7zJTJ4}Y5@e_2%5)2~}beaeRP2k_Wo$FB7t(||MBCkUIV)R_cxJu`zsq@z;2phhN@t4JOHT~Zlo`2FYF@M$j zSF}G`g@DGUoe}VkJwhGq^T>o7DDUgr8cfYdy ar@;UEcyxd`xIYo_(Z3Az-Acas^XfmEzDf}Q literal 0 HcmV?d00001 diff --git a/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg b/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg new file mode 100644 index 0000000000000000000000000000000000000000..f51ef8b73e3edf3c1d878f74414d9c99cddff3d4 GIT binary patch literal 10939 zcma*Nbx>YQ_a#hlcZcBa?ykWdg1c*Q5AOQl9^BpCd2n}khu}{5a%bv&Z|?nmGc|o` z*XjOauf2|R)$UqLQ3ebg9Rvgf8YChBOOq7~8OHASD&%*d{0?zRNqQ-H2?a()ng2cE zk77pbV}b{9>+T~AQ{yo+J_CXGqhaQ$6lcZOu#=wO3REpU+c>FGcHi&I#nP|yA!%|B zz$+K)xMiWF-2tHCx8;UtxmT2(ugz>9Z_&lPSMjRAuy%hWXwlm8k+T-|qEA|Y6Dt0LTsa~rvh8nDGr!|6%Ks&&qM)WMDz3ud?CuQb=;D0svM?sMLnBX5O+QUr zvJWdw$M9v8G5m2vc2?3TUuFL;Kl6NPeKt-(ctApM zbVXRCEv4$MA|h`jVrL|0XKZB=pk!ri$8H`4yahaE&ezT3$DxA!lU@Sk5)0UGTAzN$ z=RfJ0m^zr+nV8xcd+6I(+F3jO#d8nzU)een(5#5Rv*3RR=AYTl_73_srmm(o|LgJo zewN*TBm7zaAK%W?(t+N|(9HB7HQB-uGjp{s^G;NT&mz&cOf>-{G6 z8GJA-I*M}R^mKY0h@6tDgX-@)#s|Dx(??`i+Dd0uR+i>Z9%QvUlK9(kFZ<2c4#wMi znxE%*;J><(FE0Bpxf`5`b?(-OCO>1$KkziI@%lj;x*3@rA+~q#KNG{gR0=cwqHTEn z`bgircMr7nq19P$H9m?-02U!>c6n6z9&hjT!TfLrv!CTr<^T|h)u z2G2pf%p*XQYv1JHeahSzS^(kI{j8Z46B{TbSpWq|Nk{d993hpU==T=Jbkf+)j8q49 zCo?sOPy77Dh5y7*Z+SBx{w%D>-Vg-NEBqmsD2jdC$GJ=SOI_OZah_??JD$z8iFLaR&f zoo=g{Au+IlWMTe#)jTq{tR`tl?;ACQO>vXdc(qLhKUO{*NvkN7Jkyk9F8lG2Vb&R| zT)K0Q?lgK?>S|>8W+o+!9-^xZo=|YO(y{6G_?zEdM z6}nR5L+A9ZTszwLv*oVIE(jTHx0~pu_624v&tgjp#h)tlB}Jc!?vIAZ!{R1*bE31? z=OvrF2z_sW6Gu6Ae7;N_u0DGQTeBh~l<}WP(2bSf4^QXjRzD?yNQ092QON~lHKeYr zE{a%v`$1N9I@4NiRge`#KDLvRB9T)WBcQwX z^r#!Qva_((uOP6gqd5|*Bnr>?m0*A1s)+<>1<}F18v9uQPlG&oAN^r!@LJ@qaXmZm zHgahBscrqKHkCDxQNpA7q}KBN`kL}?43#q5SFmlI9UwRojst^{X{>4$ohX4ER) zHE6I}(h%0tqP@4?XFZRW=hsl97vHP$mnbDpBmzMKZgOZtxzc{X#`Rl(ujTN2pUh}m z$ODZ||I2=rp$~{PV$QV$lbNEa5P@EI>#rF>j}i=I-WuyLAy=q(%!hkh1AcO#7Rl9x zf2MG8*rmo4fhfjg>}9h3Bc>>ptFz9J{+OeZm_xW-0aBokuwdDgdAy#G2}wc^M25yl zMPLkQn`21+%?aW}xkTy|;?>YD;WwBbksJ?WYeo~N+ycSPiHX1u!DbUD@N1Qxdar({ zW)8+*S#KV%J6D6FSEFdXGC<66m7|NA&S3>`h)!nuM@Wooj`>_>jNwEeI8vDxc*KDRSNbG*t?TW%H8hi9+OHI*EALrZnYo=~i8K zx2q=}W3sqry;FRHHj}Y9fJa>-If4$lYvg)9Q-lV;Y9JZ+9zYBRHW=(em7;0{WtUEk z`Cm41o{(j;%jbL&HVK{tTt(}9SP=NeM?@zmR`=G9YPuy4FTZFeC^IFNDI>5+5ry|R z3~OGIELY-N$;NXg8rb;zdvz_;`ipYg$zF-lo4A3Q?d!4UQP&ajQ0*5eh*JUaR8o8M zHhvm+ZyTh&g52cdXm8wic`e>gw#RdXS+KU!CR`k2J&BCfzfCf82I&x(+~k}}%0tOj zud&ku!cd%&uI{&*B+#3#1pPxfKuNyH0PA)eN1SA_~b;n%}Uo|u4`AxzQc;_ zC8DRyF~#NPx5eP{q|eSY#60An9RE;txpHhlv{PD;e`Jt-T!CciHy9pe2ZWtp za%-{0L#K%ckO#=LWyji>v_tnbU}dQ|u{(k;m>#J+I-KCH7OAtM%R5PA&y0qzSa&c8 z-=hXlMfe9^E2|}wi-VRI3_IhiJ_|mQh>Ety2kI{3hp|SIBmt%|Wo)cMsCmkni1(q9 zTKoh)S1H-A;q}bxQN`(-Q<DvYxQJRm>a}G!KGjx1MxoT%@@iPMp*<# z2%H+9AHqO=3wR^xJ5zn?5LJh7t%JB0H{mQf-!cGRPhy3d;z`KP_{$@{ky9>3!T&O) zPcH)d+-8t;E4E_zFlQ+6HK_5Y>$fpHWgdvg$tn~K#JwtZ2N877l7p`o^>_S!aI_lj z{WP#1yMBXjamfrjbTqi~CCUJjWT{S@EDPyo($a`pEE0Yhz!zgxC%M7eP%1{R5@N+` zHCflA+Tb>{;Q1u`esrz*rY7mS&q~R@cDkh6?wGpk?shuub7OeN{=$X2A2M)QD|zIj zyUSPtf34kCqC+1EbyGzz_Fo8Hsi#>ImNd^0;VTvq zR%q8(f#+>v2My0(N5TM{4REnTl)PkuPtb!yzgQbvXps~^#?FsT zbFn*aK)iaXNi`#9lzzlY7F+)2GMwRZ z4$-oMAXGJ)`b&S1OypbN!;hmHLW7cF8JTT}o0k-K82sBNUa8uF2{+}-)dqvK5ERVfsNJoH)PJBxC1v9QmWJFw zs`FuDa!rrEg1AaP?)YK#&rGG_De+YTMl|3uIC1l#vV(kzeKZTqD$H()*k3%R8xZbP z1Iv6t>Y|M)`X=m+I?11XqW0yB14Qus^pcvyM`7H;VQulOr$RaH5ORELf1MwcW97s$ zY>_MAs&^A?Y6`y`&jjv_M(#vcOO!7~X?(QGn?8$;Q4FV}>-Q}sTPMr=Dzg-c6r9ie zEVeec0M-dx@!FH?%Z?n)+1LQlmaXWu*FnF_$ks)IK?BquaQXU;$@uQ``0kmfM0UR6!YFKVS$U`WhcGNlZq?G#=^2%YG>yC~gO%dQ#(Q-}ST{cp4(Oqs1y=3r> za#0d`nN7hZjW9%{#PoFVHl9WDc-%0g@2Z7%?Q~u>y3D8vO0>_W^pp2Wt&^e!H>iPg z(=-`Vj43;YRrq=yb;3NwIGp3;q)llP&Zj#B=#1K z^jRhbV_X+?(cJl^6)tQ zFe8nUAMDAKNs)OXxcPnx*uM@l!&f%?_ShgGFJd4dtp7i32M=RIV+&J#{eK)Xj2%3j zE$r>+S(%xCmo;9{*0#aZ@XRLW?uZeK`J!9eZlalJx&rV{vDIv{0mkGHd~1@*w9%9n z9gDIYB>;3;l**+SQ7t)WuQJuhp60FFv=!o+k8NBAu8G&KJQP~E`QsRYpn*XnE82YO z9vNoXJ|yS=NZZk&noYhaI9ATjF`s;YOZoYB)Yn~J?u2?w`ta)wzB7gBSLC!0-RL9M z_psqdm}G+ys&87qC=?Q&)|^*^x+jV!Doz>(#qi0$-a>&}KgWj?39u)X^T_R>!xny} zRj#1(h=5)u=ouRilna|Y;T2Bijqk~VMw@1`N__M4qd;jC3M{5xy!oUWlFnsoTp6~i ziI<1C7*mqqLUd}B6*gM|H^Y-G(U4s!IzO>UdTO{4#!>+_1DPy5KgRK+%7c8ZJohPh zC3qlHbBVeB<^v*H4;Qq(v;t^TdE9CXfEw;V#M~u)%?kVTHulNjQ-h5y9Eb?R%>=2TpjM zw%N)TK4#SD0e}WasL1;I%ICGFprfN^s(kqDu<_G=QmiWJ``aBc?R$zWF3T)5mWoI%JB0+yjS~+qm%5d?dGunxBTOc;W@l+znzJA zT(p{9!$pF4O_}{LtDGPyIMOEEc~}@xV{Rh?^Zv{|zGIoP?+42dqQngDHs{mxcX#Cg zxszX?0r`gD()%f4!q`%G}FQ~IJ2 zhm-Gis<51H+Y4r=1m@nbo=9;8Ch#g^e1AgR(|L$%t%97`z})ADz)u|Om=nF7b?T&*h-*&96Q zW54xLIPp;W#5bn>x!_m`B%DHXFT50XLP-EMb>G+L-~mvqs2YGMVl9fi<4EsQutVyg z<9-iPm^KDV8LMF{j4cHn@ZA4xOJzWU6x^-ZM6#N`FvYR*c^a72I;rd>Ii%#+YznOI z%Tjg2_>Wwk!OBj?dhK_TCATVO-4V6DE`@g1Yj z=LUTnwDo5eNtizBcw%)(^t&{pXC#R%YWSK7GDAm3k{gtKKDt^Bks6b~$PeVOt;}gn zi^92bDhU@n-%KM#p1~gk<=BfjcC-=Ear!>@7>kIS3OyBZlD3EwLrcftoQNj8akfE5 z!wVQa{)6wzEVV!6cCFB5>91qaF-n}k)#3XfXY6U)+TGnWO6xUz1z}2C!$rQ=+CyzM zgOx)o17ap+t)&)%r#haSgL>?kYcd`$J@m5tcDe))(jC#domo|UBnlO;x2y8dzo3j8 z%-m9tX~_|f|3GHuRS&NlO^uI;`x0Dp#pL;Qo()dAJQ(sY(Nx#siY!;KCvV8&ij|02 z4AbW;M{(bJA;g9%7bKwAgpG+9|A$EJv&C0RmohpR&13n>5`ETpmiu(!}wY;A* z&_~!!OUJ=cQ`S;i-7lnMNLak=MwKX@66u{F$F!HSkq>#N1R#KQg40tr(SNI zTCdQ^J78qEh^9qJ_;oC46ne8ndZ^(kY+)(hb|?Zc(}byG zBm)$$rlg10XOuwK=$Hj$LJWlXwDZ{hgDe&&kd zI`(${`tCNmw2aE(uRHJIMqZhu)RDHgRM%)`5oOFWTk``&Ip#SwDO-24Z#_onnOi*z z$0Bq()B>gqO+$bqb0xN?#C5~n+Uc>1Qs>&nGIU}X^|0#}8X&WHjP1%rD?*3cRjW96 zh zrD#Pem&nMS=ffK0Hzi^O-ciG=caCWXB)8^hwGMnS^fk@t=TRQNz)#<*NW@aCqN39j zlgH1&aXeXkmKX{di3Pbt`_`Ts$4>!87g!G8vA1qXY%fB>t zChynSJ_t&6#la!d|H`H@oRF2Qr>w^^9e!Umc{9n_s11EGH!JHJs+!ZcV3DYr&tA(h4T`uocNg)kyt;cBblGH4f#}l{KDP6=XpXLCIx)$^UFz3P7 zYLBWE+N4bdS83N2aKPP#eTltlgKC1REeL5wg)kev@gk=#S5ju-BH;6WN;<9E)zhcN zRWMB(#0+$BrWWLgFu$`#pQ6H?!t~#ynbr2#0K+q~&!hOBQ((j@Ee_fE>|n%$s^6{C z(K6qhmn%k=O%*w0j~vZ>eI#)b^*wqP2RMs^am*Y~u;-$n$;0}IFf(94=mw&`4^TWQe@2^E&)Xzg6 zW!sr}AD36ngWJ1|$&N)YY9kvnvcE=426{p%ZJ4#o(5%_45=f2B1>l9qwAxrt$g!P! zs`LKnoi1y%DN}2c7{k=nyj4jqbT*||P~q5_y{TE0rb_uFC~WQV0#g+kv}c-?ZAq9v zlzYQbVE%B^&d037pn&jz6TVB5(wDiPdeE0UpjjXQ@X3a&jd!3UUU16AZg&tUp-SEl zqZGkk92DPbHaaqbOEKQqVKwh_3b*r{izodQx-XFy*dniKP2Q|kkE*0!rSI{Gqpf6w z(^zTnX0!k1!N%EZRy5Fo-Nn<6yt`~%3mze(_vOo`7$6b?YcdliBu{Bjy@e{*_D9RU z{O*d7BQ2v1Kr~@NL^kg#q(zl8WuNO9W7Dj3U8fXDA7+XiZRdxMXRSCZrEx@v*%LoimZI>oxmVUa=q+owv-DY*;o-*IQ!hLuUD~&2E}ph|S{zOtx{} z5w^ZBQGWd}{Pj5U@+*c3t6+YcM>$3>F*B=e?SZ~r{XxGhKfz-?|K zPsFQ(;<^q0JaeK)J7Q}0hwyA4@8{>};&^*pOuh{9DYL&?E?2kww9%P-6uxhdSe9&- zNMUy%3P_1>8z(1cl(SFvNg^Hw1h_aM^zpTsgj;BLJ^=`Y_ zV9ftr(ob)FBu&HKDtB|tzEfzQHdN4T}D4OTf`?}(5zwjcQ(Xgd1 z?p$7dyxe9svy2^80(^uTkgg8QFFIkhJ?IEC4Cro4stHM}Q5HvDSRrkLB)~!wLfq!P zaRAo33Z>|c$3d)OgN*Oe-*5a*RbGMV#5pK{qHy~U@XF_KH`TbY6H2g30^0OlWZ7mb zpAwMs`-R#s+aov6)-2V<6kv}PR+cK{ba+}7umct0>}sd$p4Q#J<`TsTJOXB-bB(r) zQ5Hbjsng$Dn~GC>cWBv?xXPjpCR9mysoiYa(28M*p!|C*8K7ctq0Av1lfxam{8Z}5 zNT3HkCC_A{MNEXY*2hI$T#Do4#fJ1_nc#tY_k?I{Ha#Q8p_|l?KGC&aaXOkVUtx?8 z#MBB~2$7IlbA01kCe^iBpldOT2$1o6=Ic5YL0QnS)ZwUSxG!4+^4>n3JjkT4zm)<`*LVtw>Vv1 za_8E*y$q^_ICxb{YFHzBH2OQtk0??udx?87>ie3c&Up!!#k%*AdCkdSE=%ZO4QL|J z{BqExv112We^1oB|6)rGTNFqR1o(_#dLh`W(@+lsi{}&bpBVhoK`o_62@;KFtmcwd zqLit8c&*ax5{(v`^NZY-BINWl(v3_n>`AlfSEe&Wzdz@P*jvf0sKfw4(`n*q{LGZN z_l5a685eg(f{i|p61N1q&?zy|53X}{YDzKQE-NRWuz~=DwSr$uaf$RW;xbv!{PKCt z-GQQqi5C>JG^Vl61@mKV$)*JFB@lFMV}`mV|}(UC6!{TMO+ za{D9(!%f4k;rQ}sB&?kl`a0h&wH#oJdS6?roiSJ zWpUDrV$*^Y#Ff>x%RR`m`@q~9x$MsyVKzOCYPgee$hGHRZ8ks~p|(P;s&!mSJACfc zHp`FCwu}K4&N?-6<(JYfZcsa++~R6^Q}?zZ%q3^jWrz!Nc_D`T!G*<+#&ob&2KwR; ziq2m_LLHa+<121R+LudjCCljiypW-G zbI|-gb=RYIVFu#WNq@!9iggkV8j>^Gqi7&2UkjEB(Xl(}(9LO2{fVL#GMKyruXX}p zgU!y$GpCHrk!IC)p(;`;-%vYxp$A)zrZ*1NyoR`PgrfHuClpLj@Jr^;EzU7lEG4D3 z!u9#y?lsa>rG9Z{HHcVQa=Y7Iu|~psP4`g{92F=o6Ugqmm{c-%FWWeM-)Ku!zH*#S z%jianxSo#;JdWjKj+kS^?l$O-lKELPw$uc%wQi%kp86Ah58_4 zxg#sO@BY9$bFMr4CuNc;2%UvEStuP&XpkoNmnBl(zL?>W8iGuh*;-xJJY#r znPyWqh>$b(o?CX5w6|NMOb`LKEHigW$3=TFj{pjjAJ!gT>5T`InH}UVnMKBemRWlD z%v-&-6WXTwf=_Zf>7ryKN$0woewAf^&pMbI2*yI)g@fthrFNzqyfiudT51eZ#wRrD zY&#JxGFGt6GO}lmGw}&nFV)*Dd}q7ugXgOu$@W@)_v&^prmey8?Nv9|C2~1JJ(sK= zE@501M(w)KXY0!hC!qW5F?A-oH?R1XJb@t6L|m;>@3WSUj;#kI(s1bx-*eQO7Ng+% z5F||_g1TFcTwawes;BI@9g9 zi`WI^P@IBz`+Y~G8f01skO}Q3CcvZVMvW=}n!N6QJpy@@+vNUWnTvpbnK3!52=T0Sjb;#|3{%o8^3LAFG@Z!3K_vF331@*kNiRx`1 z+12c^wMI^;wXx~W?Sv1@Nku!Jt#^mGKLfirrPr~NbTWH-c{=j><7z{sdb;(Z_4Joq zA+SqZ_(qFPZ{yXStlF~^f~mhHIanXr`l0@z1!OXycM{{Qx_nrnM}m9k6RNx~ae<)b z(vf(wakj^_CP%to_!`6xbnffKQlX;iM+W0%^$&z8FgK2p99>8wkX+bpx){o zcx0bSJ+Ov+iclCt?34<`Jy=hCbs}N{y=3%nN0*3-NKm@usA(yMjYMJV<=QnIKLA9^ zJ1w~uxS5EoY-X6Vm^;c|F2rUy%8hk3X|uM@=zO_Ci0JCQ+X1YlkXO?eVeMejZ8BSp zKCIh}HK_T7zQ*&XC7g_%yfhMg8MNZCa*Y(Zl%1*?z=UgNO)+2i8JC)NE0dpF4z(6_86A4eGC zl`howas&XPx{j79-?zoamN;%m3eXCm5*8$g4@!aTb_OAUsz zG?334&pdSPzfVi^%XlR%p#nF-gzgH2eC)Y?i;k_h;$YsS>pLUk+cOgD=K%Wa_xh+~ zc3zHZw#fcMYkMXXJ4mW`Z9yxF^F&&xmo6#DDd5N(6iNGfRw~d!QWSTIfUG=1TL2b7 zV`)TG0Pq)pF5Ty{%;niWO&76%T7W-54}g8m%@l$BGTfXz+#l2nVT0d7URNHJ(vW=n zg_)a?!zc+!V$<7li&h_eed-;ISf45K136xeRSHn99#G*G8`6zB)1%=f9dyS3*4)R| zrO_SnBU#M4|MZTysCxlOSnInamglcpjKutI8?DM0_5ty?Hu9GZZkCMdy79MeasNA* z{xAB)e|3_7i5J?FV`GCL;8qdsa}9oX#*kz^;Dv&bpTbYzso|-kxZzu)l%iT?m1Lvf zsbzcN)5jTc`~m5*&C7wPk#K+;8M|*U|4~+6Em^j9LtufgCKMRVU`d#@AmK^yIl_=1 zEOImx<5DvU(j#;XGh-v;w3X9xvUJjuOsq2satce5v*m<@IcX`HsD#xTiqDb#6r6&E zY?KRCVQ3bx!=DC6g?7PMNlEt*AS>Zolu&pTKmV3+K7pcx{hz&nzu&|E8U_A;-TrI7 z`;Rm5-|7E$>;3O!5Rd}@tUuHL@b3K^;ZH-`UxvE>i~@hY-yXaF68N9&b^nI?x4Z6d zD9=Bk{^7OzH^!fI{_@xTXB7B%{E6`&x80xV|C8D7@AS*x>3`Vm{-KIFc>Ia;Kk0UV z None: + """Create the I2C connection and ensure all motors are stopped.""" + self._bot = Raspbot() + self._stop_all_motors() + + def shutdown(self) -> None: + """Stop motors and release the driver handle.""" + if self._bot is not None: + self._stop_all_motors() + self._bot = None + + # ------------------------------------------------------------------ + # Motor control + # ------------------------------------------------------------------ + + def set_speed( + self, + front_left: float, + front_right: float, + rear_left: float, + rear_right: float, + ) -> None: + """ + Set all four wheel speeds simultaneously. + + Parameters + ---------- + front_left, front_right, rear_left, rear_right: + Desired speeds in the range [-max_speed, max_speed]. + Positive = forward, negative = backward. + """ + self._require_initialized() + mapping = [ + (_MOTOR_FL, front_left), + (_MOTOR_RL, rear_left), + (_MOTOR_FR, front_right), + (_MOTOR_RR, rear_right), + ] + for motor_id, speed in mapping: + self._bot.Ctrl_Muto(motor_id, self._to_driver_units(speed)) + + self._speeds = [front_left, front_right, rear_left, rear_right] + + def stop_all(self) -> None: + """Immediately stop all motors.""" + self._require_initialized() + self._stop_all_motors() + self._speeds = [0.0, 0.0, 0.0, 0.0] + + def get_speeds(self) -> list[float]: + """ + Return the last commanded wheel speeds as [FL, FR, RL, RR]. + + The Raspbot driver provides no encoder/tachometer read-back, so this + returns the most recently written values. + """ + return list(self._speeds) + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + + def _to_driver_units(self, speed: float) -> int: + """Scale a speed value to the driver's integer range [-255, 255].""" + if self._max_speed == 0: + return 0 + scaled = (speed / self._max_speed) * _DRIVER_MAX + return int(max(-_DRIVER_MAX, min(_DRIVER_MAX, scaled))) + + def _stop_all_motors(self) -> None: + for motor_id in (_MOTOR_FL, _MOTOR_RL, _MOTOR_FR, _MOTOR_RR): + self._bot.Ctrl_Muto(motor_id, 0) + + def _require_initialized(self) -> None: + if self._bot is None: + raise RuntimeError( + "MotorController has not been initialized. Call initialize() first." + ) diff --git a/raspbot_v2_interface/setup.py b/raspbot_v2_interface/setup.py new file mode 100644 index 0000000..d4275b1 --- /dev/null +++ b/raspbot_v2_interface/setup.py @@ -0,0 +1,11 @@ +from setuptools import find_packages, setup + +setup( + name='Raspbot_Lib', + version='0.0.2', + py_modules = ['Raspbot_Lib'], + author='Yahboom Team', + url='www.yahboom.com', + packages=find_packages(), + description='Raspbot drvier V0.0.2' +) \ No newline at end of file diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..e69de29 diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..4a9e3d1 --- /dev/null +++ b/setup.py @@ -0,0 +1,16 @@ +from setuptools import setup + +package_name = 'my_robot' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + install_requires=['setuptools'], + zip_safe=True, + entry_points={ + 'console_scripts': [ + 'motor_controller = my_robot.motor_controller_node:main', + ], + }, +) \ No newline at end of file