* update demo projects

lxowalle · lxowalle · commit ee3133ef6b0d · 2025-11-17T20:31:44.000+08:00
diff --git a/projects/demo_block_tracking/main.py b/projects/demo_block_tracking/main.py
@@ -8,8 +8,8 @@
 SERVO_PORT_NAME =  '/dev/ttyS4' 	    # 舵机串口号
 SERVO_BAUDRATE = 115200 	            # 舵机的波特率
 SERVO_ID = [1, 2] 				        # 舵机ID,依次是pitch，roll，yaw
-SERVO_MIN_POSITION = [1650, 0]          # 舵机位置最小值
-SERVO_MAX_POSITION = [2446, 4096]       # 舵机位置最大值
+SERVO_MIN_POSITION = [1650, 787]          # 舵机位置最小值
+SERVO_MAX_POSITION = [2446, 3496]       # 舵机位置最大值
 value_per_angle = 4096 / 270
 SERVO_ANGLE = [0, 0]                    # 舵机旋转的角度范围
 for i, _ in enumerate(SERVO_ANGLE):
@@ -31,8 +31,8 @@
 blobs_area_threshold = 500              # blob最小面积
 blobs_pixels_threshold = 500            # blob最小像素点
 
-pitch_pid = [0.45, 0.00001, 0.000, 0]    # [P I D I_max]
-roll_pid  = [0.6, 0.01, 0.000, 0]    # [P I D I_max]
+pitch_pid = [0.5, 0.01, 0.000, 0]    # [P I D I_max]
+roll_pid  = [0.8, 0.01, 0.000, 0]    # [P I D I_max]
 
 manual_wb_enable = True                  # 使能手动白平衡，保证找色块时颜色不会随色温变化
 wb_gain_list = [0.0682, 0, 0, 0.04897]   # 手动白平衡增益参数
@@ -75,7 +75,7 @@ def rgb_list_to_lab_list(rgb_list):
 
 class App:
     def __init__(self, gimbal: servos.Gimbal):
-        self.cam = camera.Camera(640, 360)
+        self.cam = camera.Camera(640, 360, fps=60)
         self.disp = display.Display()
         self.ts = touchscreen.TouchScreen()
         self.img_exit = image.load("./assets/exit.jpg").resize(40, 40)
@@ -154,7 +154,7 @@ def run(self):
                     obj_x = b.cx()
                     obj_y = b.cy()
                     
-                    print(b.cx(), b.cy())
+                    # print(b.cx(), b.cy())
                     for i in range(4):
                         img.draw_line(corners[i][0], corners[i][1], corners[(i + 1) % 4][0], corners[(i + 1) % 4][1], image.COLOR_RED, 4)
 
@@ -163,7 +163,7 @@ def run(self):
                     center_y = img_h / 2
                     angle_x = (obj_x - center_x) / img_w * CAMERA_FOV_H
                     angle_y = (obj_y - center_y) / img_h * CAMERA_FOV_V
-                    print(f'angle_x:{angle_x:.2f} fov_h:{CAMERA_FOV_H:.2f}', f'angle_y:{angle_y:.2f} fov_v:{CAMERA_FOV_V:.2f}')
+                    # print(f'angle_x:{angle_x:.2f} fov_h:{CAMERA_FOV_H:.2f}', f'angle_y:{angle_y:.2f} fov_v:{CAMERA_FOV_V:.2f}')
 
                     # # 偏移的角度映射到舵机偏移的百分比
                     # pitch_error = (angle_y + SERVO_ANGLE[0] / 2) / SERVO_ANGLE[0] * 100
@@ -173,7 +173,7 @@ def run(self):
                     pitch_error = angle_y * value_per_angle
                     roll_error = angle_x * value_per_angle
 
-                print(f'pitch_error:{pitch_error:.2f} pitch_angle_range:{SERVO_ANGLE[0]:.2f} roll_error:{roll_error:.2f} roll_angle_range:{SERVO_ANGLE[1]:.2f}')
+                # print(f'pitch_error:{pitch_error:.2f} pitch_angle_range:{SERVO_ANGLE[0]:.2f} roll_error:{roll_error:.2f} roll_angle_range:{SERVO_ANGLE[1]:.2f}')
                 gimbal.run(pitch_error, roll_error, pitch_reverse = pitch_reverse, roll_reverse=roll_reverse)
             
                 if self.need_exit:
@@ -194,59 +194,52 @@ def run(self):
     pid_roll = servos.PID(p=roll_pid[0], i=roll_pid[1], d=roll_pid[2], imax=roll_pid[3])
     gimbal = servos.Gimbal(s, pid_pitch, pid_roll)
 
+    def print_pid(s, id: int):
+        print(f'ID:{id} position p:{s.uservo.get_position_p(id)} i:{s.uservo.get_position_i(id)} d:{s.uservo.get_position_d(id)}')
+        print(f'ID:{id} speed p:{s.uservo.get_speed_p(id)} i:{s.uservo.get_speed_i(id)} d:{s.uservo.get_speed_d(id)}')
+    
+    def print_max_min_angle(s, id: int):
+        print(f'ID:{id} min {s.uservo.get_lower_angle(id)} max:{s.uservo.get_upper_angle(id)}')
+        
+    print_pid(s, 1)
+    print_pid(s, 2)
+    print_max_min_angle(s, 1)
+    print_max_min_angle(s, 2)
+
     if test_get_position_enable:
         s.test_position()
     elif test_set_pitch_position_enable:
-        s.uservo.set_position(1, 2048)
-        while True:
-            pos = s.uservo.get_position(1)
-            if pos:
-                if pos > 2000 and pos < 3100:
-                    break
-            time.sleep_ms(1)
-
-        t = time.ticks_us()
-        s.uservo.set_position(1, 2548)
-        while True:
-            pos = s.uservo.get_position(1)
-            if pos:
-                if pos > 2548-50 and pos < 2548+50:
-                    break
-            time.sleep_ms(1)
-        print('cost', time.ticks_us() - t)
-    elif test_set_roll_position_enable:
-        id = 2
-        angle = 120
-        start_position = 1024
-        dst_position = start_position + angle / 270 * 4096
-        s.uservo.set_position(id, start_position)
-        
-        while True:
-            pos = s.uservo.get_position(id)
-            if pos:
-                if pos > start_position - 25 and pos < start_position + 25:
-                    break
-                print('wait start position, dst position', dst_position,'curr position', pos)
-            time.sleep_ms(100)
-
-        t = time.ticks_us()
-        s.uservo.set_position(id, dst_position)
-        print('run set_position cost', time.ticks_us() - t)
-
-        t = time.ticks_us()
-        while True:
-            pos = s.uservo.get_position(id)
-            if pos:
-                if pos > dst_position-25 and pos < dst_position+100:
-                    break
-                print('wait dst position, dst position', dst_position,'curr position', pos)
-            # time.sleep_ms(1)
-        print('wait cost', time.ticks_us() - t)
+        id = SERVO_ID[0]
+        while not app.need_exit():
+            for position in range(SERVO_MIN_POSITION[id-1], SERVO_MAX_POSITION[id-1], 10):
+                s.uservo.set_position(id, position)
+
+                pos = s.uservo.get_position(id)
+                print(f'id:{id} position:{pos}')
+                time.sleep_ms(10)
+
+            for position in range(SERVO_MAX_POSITION[id-1], SERVO_MIN_POSITION[id-1], -10):
+                s.uservo.set_position(id, position)
 
+                pos = s.uservo.get_position(id)
+                print(f'id:{id} position:{pos}')
+                time.sleep_ms(10)
+    elif test_set_roll_position_enable:
+        id = SERVO_ID[1]
         while not app.need_exit():
-            pos = s.uservo.get_position(id)
-            print(pos)
-            time.sleep_ms(1000)
+            for position in range(SERVO_MIN_POSITION[id-1], SERVO_MAX_POSITION[id-1], 10):
+                s.uservo.set_position(id, position)
+
+                pos = s.uservo.get_position(id)
+                print(f'id:{id} set:{position} get:{pos}')
+                time.sleep_ms(10)
+
+            for position in range(SERVO_MAX_POSITION[id-1], SERVO_MIN_POSITION[id-1], -10):
+                s.uservo.set_position(id, position)
+
+                pos = s.uservo.get_position(id)
+                print(f'id:{id} set:{position} get:{pos}')
+                time.sleep_ms(10)
     else:
         a = App(gimbal)
         a.run()
diff --git a/projects/demo_block_tracking/servos.py b/projects/demo_block_tracking/servos.py
@@ -18,9 +18,12 @@ def __init__(self, id_list:list, device_name:str, baudrate:int, min_position_lis
         self.uart = uart.UART(device_name, baudrate)
         self.uservo = UartServoManager(self.uart, servo_id_list=id_list)
         self.id_list = id_list
-        for id in id_list:
+        for idx, id in enumerate(id_list):
             self.uservo.torque_enable(id, True)
             self.uservo.set_position(id, 2048, True)
+            # self.uservo.set_lower_angle(id, min_position_list[idx])
+            # self.uservo.set_upper_angle(id, max_position_list[idx])
+
         self.max_position_list = max_position_list
         self.min_position_list = min_position_list
         if len(self.id_list) != len(self.max_position_list) != len(self.min_position_list):
@@ -40,7 +43,7 @@ def set_pitch(self, inc:float):
             position = position if position >= self.min_position_list[0] else self.min_position_list[0]
             position = position if position <= self.max_position_list[0] else self.max_position_list[0]
             self.uservo.set_position(self.pitch_id, position)
-            print(f'set pitch inc:{inc:.2f}', f'position:{position:.2f}', f'range:[{self.min_position_list[0]},{self.max_position_list[0]}]')
+            # print(f'set pitch inc:{inc:.2f}', f'position:{position:.2f}', f'range:[{self.min_position_list[0]},{self.max_position_list[0]}]')
 
     def set_roll(self, inc:float):
         if inc == 0:
@@ -52,7 +55,7 @@ def set_roll(self, inc:float):
             position = position if position >= self.min_position_list[1] else self.min_position_list[1]
             position = position if position <= self.max_position_list[1] else self.max_position_list[1]
             self.uservo.set_position(self.roll_id, position)
-            print(f'set roll inc:{inc:.2f}', f'position:{position:.2f}', f'range:[{self.min_position_list[1]},{self.max_position_list[1]}]')
+            # print(f'set roll inc:{inc:.2f}', f'position:{position:.2f}', f'range:[{self.min_position_list[1]},{self.max_position_list[1]}]')
 
     def set_yaw(self, inc:float):
         if inc == 0:
diff --git a/projects/demo_block_tracking/src/uart_servo.py b/projects/demo_block_tracking/src/uart_servo.py
@@ -237,6 +237,82 @@ def write_data_by_name(self, servo_id, data_name, value):
 		param_bytes = struct.pack(f">{dtype}", value)
 		self.write_data(servo_id, data_address, param_bytes)
 	
+	def set_id(self, new_servo_id):
+		'''设置舵机ID'''
+		self.write_data_by_name(0xfe, 'SERVO_ID', new_servo_id)
+
+	def get_id(self):
+		'''获取舵机ID'''
+		return self.read_data_by_name(0xfe, 'SERVO_ID')
+
+	def set_position_p(self, servo_id, value):
+		'''设置位置环 P参数'''
+		self.write_data_by_name(servo_id, "CONTROL_P_KP", value)
+
+	def get_position_p(self, servo_id):
+		'''获取位置环 P参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_P_KP')
+
+	def set_position_i(self, servo_id, value):
+		'''设置位置环 I参数'''
+		self.write_data_by_name(servo_id, "CONTROL_P_KI", value)
+
+	def get_position_i(self, servo_id):
+		'''获取位置环 I参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_P_KI')
+
+	def set_position_d(self, servo_id, value):
+		'''设置位置环 D参数'''
+		self.write_data_by_name(servo_id, "CONTROL_P_KD", value)
+
+	def get_position_d(self, servo_id):
+		'''获取位置环 D参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_P_KD')
+
+	def set_position_p(self, servo_id, value):
+		'''设置位置环 P参数'''
+		self.write_data_by_name(servo_id, "CONTROL_P_KP", value)
+
+	def set_speed_p(self, servo_id, value):
+		'''设置速度环 P参数'''
+		self.write_data_by_name(servo_id, "CONTROL_V_KP", value)
+
+	def get_speed_p(self, servo_id):
+		'''获取速度环 P参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_V_KP')
+
+	def set_speed_i(self, servo_id, value):
+		'''设置速度环 I参数'''
+		self.write_data_by_name(servo_id, "CONTROL_V_KI", value)
+
+	def get_speed_i(self, servo_id):
+		'''获取速度环 I参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_V_KI')
+
+	def set_speed_d(self, servo_id, value):
+		'''设置速度环 D参数'''
+		self.write_data_by_name(servo_id, "CONTROL_V_KD", value)
+
+	def get_speed_d(self, servo_id):
+		'''获取速度环 D参数'''
+		return self.read_data_by_name(servo_id, 'CONTROL_V_KD')
+
+	def set_lower_angle(self, servo_id, value):
+		'''设置舵机角度下限'''
+		self.write_data_by_name(servo_id, "ANGLE_LOWERB", value)
+
+	def get_lower_angle(self, servo_id):
+		'''获取舵机角度下限'''
+		return self.read_data_by_name(servo_id, 'ANGLE_LOWERB')
+
+	def set_upper_angle(self, servo_id, value):
+		'''设置舵机角度上限'''
+		self.write_data_by_name(servo_id, "ANGLE_UPPERB", value)
+
+	def get_upper_angle(self, servo_id):
+		'''获取舵机角度上限'''
+		return self.read_data_by_name(servo_id, 'ANGLE_UPPERB')
+
 	def get_legal_position(self, position):
 		'''获取合法的位置'''
 		position = int(position)
diff --git a/projects/demo_subway_surfers/.gitignore b/projects/demo_subway_surfers/.gitignore
@@ -0,0 +1 @@
+deps
diff --git a/projects/demo_subway_surfers/README.md b/projects/demo_subway_surfers/README.md
@@ -0,0 +1,47 @@
+# README
+
+通过视觉检测到姿态变化, 来完成地铁跑酷需要的动作. 支持maixcam/maixcam2, 以下统称为设备
+
+## 使用方法
+
+### 配置游戏环境
+
+
+1. 将设备的usb接口连接到电脑
+2. 打设备的HID Mouse功能, 打开方法是在设置应用->USB设置->选择device->勾选HID Mouse->点击确定
+3. 在电脑上打开地铁跑酷游戏. 可以点击[这里](https://subtlexp.github.io/Subway-Surfers)用浏览器玩地铁跑酷
+
+### 开始游戏
+
+1. 打开应用后, 确保画面中存在玩家的膝盖到头部的区域
+2. 玩家初始时需要站直, 肩膀保持自然水平, 裁判将设备对准玩家后点击BOOT键进行校准, 校准完成后设备会立即检测玩家姿态并操作鼠标.这时候在电脑上点击游戏开始
+3. 玩家的操作方式是:
+    - 向左旋转肩膀, 表示左移一次
+    - 向右旋转肩膀, 表示右转一次
+    - 跳跃, 表示跳跃一次
+    - 蹲下, 表示蹲下一次
+注意:
+1. 判断左移和右移的标准是肩膀向左或者向右旋转的角度, 一般旋转角度超过10度后认为有效. 旋转后需要立马回正
+2. 跳越和蹲下时由于身体需要一定时间反映, 因此有一定延时, 需要提前做预判动作
+
+
+Complete the actions required for Subway Surfers by detecting posture changes through visual detection. Supports maixcam/maixcam2, collectively referred to as the device below.
+
+## Usage
+### Setting Up the Game Environment
+1. Connect the device's USB interface to the computer.
+2. Enable the HID Mouse function of the device. To do this, go to the Settings app -> USB Settings -> Select device -> Check HID Mouse -> Click OK.
+3. Open the Subway Surfers game on the computer. You can click [here](https://subtlexp.github.io/Subway-Surfers) to play Subway Surfers in your browser.
+
+### Starting the Game
+1. After opening the application, ensure that the area from the player's knees to the head is visible in the frame.
+2. The player should initially stand straight with their shoulders naturally level. The referee should aim the device at the player and press the BOOT key to calibrate. Once calibration is complete, the device will immediately detect the player's posture and control the mouse. At this point, click to start the game on the computer.
+3. The player's controls are as follows:
+- Rotate the shoulders to the left to move left once.
+- Rotate the shoulders to the right to move right once.
+- Jump to perform a jump.
+- Crouch to perform a crouch.
+
+Note:
+1. The criteria for determining left and right movement are the angles of shoulder rotation to the left or right. Generally, a rotation angle exceeding 10 degrees is considered valid. After rotating, return to the original position immediately.
+2. Due to the time required for the body to react, there is a certain delay when jumping and crouching. Preemptive actions are necessary.
diff --git a/projects/demo_subway_surfers/app.png b/projects/demo_subway_surfers/app.png
diff --git a/projects/demo_subway_surfers/app.yaml b/projects/demo_subway_surfers/app.yaml
@@ -0,0 +1,9 @@
+id: subway_surfers
+name: Subway Surfers
+version: 1.0.0
+author: sipeed
+icon: app.png
+desc: subway_surfers
+files:
+  - main.py
+  - README.md
diff --git a/projects/demo_subway_surfers/main.py b/projects/demo_subway_surfers/main.py
