Initial commit

Nothing has been tested yet.

.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"]

.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/"
+}

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+  "python.analysis.extraPaths": [
+    "/opt/ros/kilted/lib/python3.12/site-packages"
+  ]
+}

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

package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>my_robot</name>
+  <version>0.0.1</version>
+  <description>Four-wheel robot motor controller</description>
+  <maintainer email="you@example.com">Your Name</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+</package>

raspbot_v2_interface/README.md

@@ -0,0 +1,5 @@
+## Installation Steps (安装步骤)
+
+cd py_install
+
+sudo python3 setup.py install

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

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

raspbot_v2_interface/Raspbot_Lib.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

raspbot_v2_interface/Raspbot_Lib.egg-info/top_level.txt

@@ -0,0 +1 @@
+Raspbot_Lib

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

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

raspbot_v2_interface/Raspbot_Lib/__init__.py

@@ -0,0 +1 @@
+from .Raspbot_Lib import Raspbot,LightShow

raspbot_v2_interface/__init__.py

@@ -0,0 +1,3 @@
+from .motor_controller import MotorController
+
+__all__ = ["MotorController"]

raspbot_v2_interface/build/lib/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)
+        

raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py

@@ -0,0 +1 @@
+from .Raspbot_Lib import Raspbot,LightShow

raspbot_v2_interface/motor_controller.py

@@ -0,0 +1,109 @@
+from .Raspbot_Lib import Raspbot
+
+# Motor ID assignments (from Raspbot_Lib comments)
+_MOTOR_FL = 0  # L1 — front left
+_MOTOR_RL = 1  # L2 — rear left
+_MOTOR_FR = 2  # R1 — front right
+_MOTOR_RR = 3  # R2 — rear right
+
+_DRIVER_MAX = 255
+
+
+class MotorController:
+    """
+    Thin adapter between the ROS 2 motor controller node and Raspbot_Lib.
+
+    Speed convention: the node passes values in the range
+    [-max_speed, max_speed] (default max_speed = 1.0).  This class scales
+    them linearly to the driver's 0-255 range, so max_speed should match
+    whatever normalised unit you use (1.0 → full throttle).
+    """
+
+    def __init__(self, max_speed: float = 1.0):
+        self._max_speed = max_speed
+        self._bot: Raspbot | None = None
+        # Shadow register — Raspbot_Lib has no read-back, so we track state.
+        self._speeds = [0.0, 0.0, 0.0, 0.0]  # [FL, FR, RL, RR]
+
+    # ------------------------------------------------------------------
+    # Lifecycle
+    # ------------------------------------------------------------------
+
+    def initialize(self) -> 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."
+            )

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'
+)

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',
+        ],
+    },
+)