import time import _thread import math from machine import UART from RemoteReceiver import ESPNowReceiver import servo # ------------------- 1. Vision Tracking Configuration ------------------- UART_NUM = 1 BAUD_RATE = 115200 TX_PIN = 17 RX_PIN = 16 UART_TIMEOUT = 2000 # Timeout for vision data (ms) # Visual Tracking Parameters TARGET_X_CENTER = 160 # Center X coordinate (Resolution: 320x224) X_TOLERANCE = 80 # Deadband range (160±80 -> 80 to 240) SEARCH_SPEED = 0.4 # Slow speed for person searching (Higher value = Slower gait) AUTO_TRACK_SPEED = 0.4 # Gait period during active tracking # Mode Control MODE_MANUAL = 0 # Manual Remote Mode (Default) MODE_AUTO = 1 # Auto-Tracking Mode current_mode = MODE_MANUAL last_sw_l_state = 0 # Previous state of Left Joystick Switch for debouncing MODE_SWITCH_DELAY = 500 # Debounce delay (ms) last_mode_switch_time = 0 # Vision Data Types VISION_TYPE_NONE = 0 # Invalid/No data VISION_TYPE_TRACK = 1 # Tracking coordinates (e.g., x:xxx,y:xxx) VISION_TYPE_STOP = 2 # Proximity stop command ("stop") vision_data = { "data_type": VISION_TYPE_NONE, "target_x": None, "last_update": time.ticks_ms() } # ------------------- 2. Servo Calibration & Control ------------------- def control_servo(num, angle): """ Calibrate and execute servo movement based on hardware orientation. """ if num == 4 or num == 6: angle = -angle # Calibrate Left Thighs if num in [1, 3, 5, 7]: angle = -angle # Calibrate all Calves (Lower legs) if num == 8: angle = -angle # Calibrate Lid/Cover Servo servo.set_servo_angle(num, angle) # ------------------- 3. Lid Auto-Oscillation Config ------------------- COVER_AUTO_MODE = False COVER_AUTO_PERIOD_BASE = 4.0 # Base cycle (4s) COVER_AUTO_PERIOD_MIN = 1.0 # Minimum cycle (Fastest) COVER_AUTO_PERIOD_MAX = 8.0 # Maximum cycle (Slowest) cover_auto_start_time = 0 cover_auto_current_period = 4.0 # Real-time period (adjustable via joystick) cover_auto_init_angle = 0 # Starting angle when auto-mode is toggled # ------------------- 4. Global Motion Parameters ------------------- SENDER_MAC = b'\xec\xe34+\xce\x08' DEAD_ZONE = 1000 GAIT_PERIOD_MIN = 0.4 GAIT_PERIOD_MAX = 1.0 GAIT_PERIOD_STEP = 0.05 FIXED_LIFT_ANGLE = 15 COVER_ANGLE_MIN = -45 COVER_ANGLE_MAX = 45 COVER_STEP = 1 remote_data = { "vrx_l": 2048, "vry_l": 2048, "vrx_r": 2048, "vry_r": 2048, "sw_l": 0, "sw_r": 0, "t_l": 0, "t_r": 0, "last_update": time.ticks_ms(), "is_running": True } motion_params = { "move_dir": 0, "gait_period": 0.4, "sw_l_action": False, "sw_r_action": False, "cover_left_angle": 0, "cover_right_angle": 0 } # ------------------- 5. Threaded Data Handling ------------------- def data_callback(parsed_data): """ESP-NOW Callback to update remote control status.""" remote_data.update({ "vrx_l": parsed_data.get("vrx_l", 2048), "vry_l": parsed_data.get("vry_l", 2048), "vrx_r": parsed_data.get("vrx_r", 2048), "vry_r": parsed_data.get("vry_r", 2048), "sw_l": parsed_data.get("sw_l", 0), "sw_r": parsed_data.get("sw_r", 0), "t_l": parsed_data.get("t_l", 0), "t_r": parsed_data.get("t_r", 0), "last_update": time.ticks_ms() }) def receiver_thread(): """Thread to handle ESP-NOW wireless signals.""" receiver = ESPNowReceiver(sender_mac=SENDER_MAC, debug_print=0) try: receiver.run(callback=data_callback) except Exception as e: print(f"Receiver Thread Error: {e}") finally: remote_data["is_running"] = False def parse_vision_data(data): """Parse UART data from vision module (formats: 'stop' or 'x:xxx,y:xxx').""" try: data_str = data.decode('utf-8').strip() if data_str.lower() == "stop": return (VISION_TYPE_STOP, None) if data_str.startswith("x:") and ",y:" in data_str: x_part = data_str.split(",")[0] x_val = int(x_part.split(":")[1]) if 0 <= x_val <= 320: return (VISION_TYPE_TRACK, x_val) except: pass return (VISION_TYPE_NONE, None) def vision_receiver_thread(): """Thread to monitor UART for Vision Tracking data.""" uart = UART(UART_NUM, baudrate=BAUD_RATE, tx=TX_PIN, rx=RX_PIN, timeout=UART_TIMEOUT) print(f"Vision UART Initialized at {BAUD_RATE}bps") while remote_data["is_running"]: uart_data = uart.readline() if uart_data: d_type, t_x = parse_vision_data(uart_data) if d_type != VISION_TYPE_NONE: vision_data.update({"data_type": d_type, "target_x": t_x, "last_update": time.ticks_ms()}) else: vision_data["data_type"] = VISION_TYPE_NONE else: vision_data["data_type"] = VISION_TYPE_NONE time.sleep_ms(50) # ------------------- 6. Logic Processing ------------------- def check_mode_switch(): """Toggle between MANUAL/AUTO modes on Left Joystick click.""" global current_mode, last_sw_l_state, last_mode_switch_time c_sw = remote_data["sw_l"] c_time = time.ticks_ms() if c_sw == 1 and last_sw_l_state == 0 and time.ticks_diff(c_time, last_mode_switch_time) > MODE_SWITCH_DELAY: current_mode = MODE_AUTO if current_mode == MODE_MANUAL else MODE_MANUAL last_mode_switch_time = c_time print(f"\n--- MODE SWITCH: {'AUTO-TRACK' if current_mode == MODE_AUTO else 'MANUAL'} ---") motion_params["move_dir"] = 0 # Stop robot during transition last_sw_l_state = c_sw def update_motion_params(): """Calculates gait and lid angles based on active mode.""" check_mode_switch() # --- Gait Logic --- if current_mode == MODE_MANUAL: move_dir = 0 if remote_data["vry_l"] < 2048 - DEAD_ZONE: move_dir = 1 elif remote_data["vry_l"] > 2048 + DEAD_ZONE: move_dir = -1 if move_dir == 0: move_dir = 2 if remote_data["vrx_l"] > 2048 + DEAD_ZONE else 3 if remote_data["vrx_l"] < 2048 - DEAD_ZONE else 0 gait_period = motion_params["gait_period"] if remote_data["t_r"] == 1: gait_period = min(GAIT_PERIOD_MAX, gait_period + GAIT_PERIOD_STEP) if remote_data["t_l"] == 1: gait_period = max(GAIT_PERIOD_MIN, gait_period - GAIT_PERIOD_STEP) else: # AUTO-TRACKING Logic lost = time.ticks_diff(time.ticks_ms(), vision_data["last_update"]) > UART_TIMEOUT if lost or vision_data["data_type"] == VISION_TYPE_NONE: move_dir, gait_period = 3, SEARCH_SPEED # Rotate to find person elif vision_data["data_type"] == VISION_TYPE_STOP: move_dir, gait_period = 0, motion_params["gait_period"] # Halt proximity else: tx = vision_data["target_x"] gait_period = AUTO_TRACK_SPEED if (TARGET_X_CENTER - X_TOLERANCE) <= tx <= (TARGET_X_CENTER + X_TOLERANCE): move_dir = 1 # Move Forward elif tx < (TARGET_X_CENTER - X_TOLERANCE): move_dir, gait_period = 2, 1.2 # Turn Right else: move_dir, gait_period = 3, 1.2 # Turn Left # --- Lid Logic --- global COVER_AUTO_MODE, cover_auto_start_time, cover_auto_init_angle, cover_auto_current_period cl_angle, cr_angle = motion_params["cover_left_angle"], motion_params["cover_right_angle"] c_time = time.ticks_ms() # Toggle Auto Lid on Right Joystick Click if remote_data["sw_r"] == 1 and not motion_params["sw_r_action"]: COVER_AUTO_MODE = not COVER_AUTO_MODE if COVER_AUTO_MODE: cover_auto_init_angle = (cl_angle + cr_angle) / 2 cover_auto_current_period, cover_auto_start_time = COVER_AUTO_PERIOD_BASE, c_time print(f"Auto Lid ON (Period: {COVER_AUTO_PERIOD_BASE}s)") motion_params["sw_r_action"] = remote_data["sw_r"] == 1 if COVER_AUTO_MODE: # Dynamic Period Tuning via Right Stick Y if remote_data["vry_r"] < 2048 - DEAD_ZONE: cover_auto_current_period = max(COVER_AUTO_PERIOD_MIN, cover_auto_current_period - 0.1) elif remote_data["vry_r"] > 2048 + DEAD_ZONE: cover_auto_current_period = min(COVER_AUTO_PERIOD_MAX, cover_auto_current_period + 0.1) # Sine-wave Oscillation Math elapsed = time.ticks_diff(c_time, cover_auto_start_time) / 1000.0 radian = (elapsed % cover_auto_current_period / cover_auto_current_period) * 2 * math.pi target_angle = max(COVER_ANGLE_MIN, min(COVER_ANGLE_MAX, cover_auto_init_angle + math.sin(radian) * COVER_ANGLE_MAX)) cl_angle = cr_angle = target_angle else: # Manual Control Logic if remote_data["vrx_r"] > 2048 + DEAD_ZONE: cl_angle = min(COVER_ANGLE_MAX, cl_angle + COVER_STEP) elif remote_data["vrx_r"] < 2048 - DEAD_ZONE: cr_angle = min(COVER_ANGLE_MAX, cr_angle + COVER_STEP) else: if remote_data["vry_r"] < 2048 - DEAD_ZONE: cl_angle = min(COVER_ANGLE_MAX, cl_angle + COVER_STEP) cr_angle = min(COVER_ANGLE_MAX, cr_angle + COVER_STEP) elif remote_data["vry_r"] > 2048 + DEAD_ZONE: cl_angle = max(COVER_ANGLE_MIN, cl_angle - COVER_STEP) cr_angle = max(COVER_ANGLE_MIN, cr_angle - COVER_STEP) motion_params.update({"move_dir": move_dir, "gait_period": gait_period, "cover_left_angle": cl_angle, "cover_right_angle": cr_angle}) # ------------------- 7. Hardware Execution ------------------- t = 0 def control_robot(): global t params = motion_params p = t / params["gait_period"] m_dir = params["move_dir"] control_servo(8, params["cover_left_angle"]) control_servo(9, params["cover_right_angle"]) if m_dir == 0: for i in range(8): control_servo(i, 0) t = 0 else: t = (t + 0.02) % params["gait_period"] if m_dir in [1,2] else (t - 0.02) % params["gait_period"] d = -1 if m_dir == -1 or m_dir == 3 else 1 if m_dir in [1, -1]: # Forward / Backward if p <= 0.5: control_servo(0,0); control_servo(2,0); control_servo(4,-45); control_servo(6,45) control_servo(1,d*FIXED_LIFT_ANGLE); control_servo(3,-d*FIXED_LIFT_ANGLE) control_servo(5,-d*FIXED_LIFT_ANGLE); control_servo(7,d*FIXED_LIFT_ANGLE) else: control_servo(0,-45); control_servo(2,45); control_servo(4,0); control_servo(6,0) control_servo(1,-d*FIXED_LIFT_ANGLE); control_servo(3,d*FIXED_LIFT_ANGLE) control_servo(5,d*FIXED_LIFT_ANGLE); control_servo(7,-d*FIXED_LIFT_ANGLE) else: # Left / Right Turn if p <= 0.5: control_servo(0,-30); control_servo(2,30); control_servo(4,-30); control_servo(6,30) control_servo(1,d*FIXED_LIFT_ANGLE); control_servo(3,-d*FIXED_LIFT_ANGLE) control_servo(5,-d*FIXED_LIFT_ANGLE); control_servo(7,d*FIXED_LIFT_ANGLE) else: control_servo(0,0); control_servo(2,0); control_servo(4,0); control_servo(6,0) control_servo(1,-d*FIXED_LIFT_ANGLE); control_servo(3,d*FIXED_LIFT_ANGLE) control_servo(5,d*FIXED_LIFT_ANGLE); control_servo(7,-d*FIXED_LIFT_ANGLE) # ------------------- 8. Entry Point ------------------- def main(): _thread.start_new_thread(receiver_thread, ()) _thread.start_new_thread(vision_receiver_thread, ()) servo.servo_init() print("\n=== SYSTEM BOOT SUCCESSFUL ===") print("Default Mode: MANUAL (Click Left Joystick to toggle AUTO-TRACK)") print("Auto-Lid Control: Click Right Joystick to toggle (Adjust speed via Y-Axis)") try: while remote_data["is_running"]: if current_mode == MODE_MANUAL and time.ticks_diff(time.ticks_ms(), remote_data["last_update"]) > 5000: motion_params["move_dir"] = 0 update_motion_params() control_robot() time.sleep_ms(20) except KeyboardInterrupt: print("\nExiting System...") finally: for n in range(10): control_servo(n, 0) print("Robot Safely Parked.") if __name__ == "__main__": main()