commit 845f61e710cdf171ff7a02e460167d2d3001fa76 Author: Matt Spencer Date: Wed Apr 15 18:12:23 2026 +0100 Initial commit Nothing has been tested yet. 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 0000000..2b20dc4 Binary files /dev/null and b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc differ 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 0000000..dc728c1 Binary files /dev/null and b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc differ diff --git a/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc new file mode 100644 index 0000000..63bc85e Binary files /dev/null and b/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc differ diff --git a/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc b/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc new file mode 100644 index 0000000..c8e66c5 Binary files /dev/null and b/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc differ diff --git a/raspbot_v2_interface/__init__.py b/raspbot_v2_interface/__init__.py new file mode 100644 index 0000000..8c72de4 --- /dev/null +++ b/raspbot_v2_interface/__init__.py @@ -0,0 +1,3 @@ +from .motor_controller import MotorController + +__all__ = ["MotorController"] diff --git a/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py b/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py new file mode 100644 index 0000000..4b101ce --- /dev/null +++ b/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) + 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 0000000..f28ecfd Binary files /dev/null and b/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg differ 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 0000000..f51ef8b Binary files /dev/null and b/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg differ diff --git a/raspbot_v2_interface/motor_controller.py b/raspbot_v2_interface/motor_controller.py new file mode 100644 index 0000000..7dff815 --- /dev/null +++ b/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." + ) 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