机器人控制程序

jerryinwuhan2025-12-04 14:01

  • 主控：ESP8266（NodeMCU 一类）

  • 舵机驱动：PCA9685（16 路）

  • I²C 连接：

    • PCA9685 SDA → D1
    • PCA9685 SCL → D2

  • 舵机通道分配（你描述的映射，我按"前/后 + 左/右 + 髋/膝"来命名）：

PCA9685 通道 你的叫法 建议名字
0 左上 前左髋 LF_HIP
1 右上 前右髋 RF_HIP
2 左下 后左髋 LR_HIP
3 右下 后右髋 RR_HIP
4 左上前 前左膝 LF_KNEE
5 右上前 前右膝 RF_KNEE
6 左下前 后左膝 LR_KNEE
7 右下前 后右膝 RR_KNEE

说明：

  • "髋 HIP" 控制左右摆动（往前迈腿 / 往后收腿）
  • "膝 KNEE" 控制抬腿 / 落腿

程序功能说明

这个程序主要做三件事：

  1. 初始化所有 8 个舵机到"中立角度"（建议 90°）

  2. 串口手动调节：

    • 在串口监视器里输入：id angle（比如：0 80 表示把通道 0 调到 80°）
    • 用于校准舵机安装角度（即把各关节调到你认为合适的"零位"）

  3. 简单动作演示：

    • f：向前走几步
    • b：向后走几步
    • l：原地左转
    • r：原地右转
    • h：回到站立姿态（全部回 90°）

你可以先用"手动模式"调好每个关节的"零位"，把这些中立角度记下来，之后可写死成 offset[] 或 neutralAngle[]。

完整 Arduino 程序（ESP8266 + PCA9685）

需要的库：

  • Adafruit PWM Servo Driver Library （Adafruit_PWMServoDriver）
  • Wire（ESP8266 Arduino 核心自带）
cpp 复制代码 
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// PCA9685 I2C 地址，一般是 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

// 根据官方例程，给一个大致的范围（可以之后根据你自己的舵机微调）
#define SERVOMIN  150   // 0° 对应脉宽
#define SERVOMAX  600   // 180° 对应脉宽

// 便于以后修改频率
const int SERVO_FREQ = 50;  // SG90 一般 50Hz

// 通道定义，方便阅读
enum {
  CH_LF_HIP  = 0,  // 左前髋
  CH_RF_HIP  = 1,  // 右前髋
  CH_LR_HIP  = 2,  // 左后髋
  CH_RR_HIP  = 3,  // 右后髋
  CH_LF_KNEE = 4,  // 左前膝
  CH_RF_KNEE = 5,  // 右前膝
  CH_LR_KNEE = 6,  // 左后膝
  CH_RR_KNEE = 7   // 右后膝
};

// 每个舵机当前角度记录，方便手动调节和动作控制
float servoAngle[8] = {45,45,45,45,45,45,45,45};

// 是否反向（有些舵机安装方向相反，可以在这里调整）
bool servoReversed[8] = {
  false, // 0 LF_HIP
  false, // 1 RF_HIP
  false, // 2 LR_HIP
  false, // 3 RR_HIP
  false, // 4 LF_KNEE
  false, // 5 RF_KNEE
  false, // 6 LR_KNEE
  false  // 7 RR_KNEE
};

// 校准偏移（单位：度），用来把物理安装误差调平
// 手动调试完成后，可以把最终的中立角写到这里
int servoOffset[8] = {0,0,0,0,0,0,0,0};

// --------- 工具函数：角度转脉宽 0-4095 ----------
uint16_t angleToPulse(float angle) {
  // 限制在 0~180
  if (angle < 0) angle = 0;
  if (angle > 180) angle = 180;

  // 从 [0,180] 映射到 [SERVOMIN,SERVOMAX]
  float pulse = SERVOMIN + (SERVOMAX - SERVOMIN) * (angle / 180.0);
  return (uint16_t)pulse;
}

// 设置单个舵机角度（带 offset 和 reverse）
void setServoAngle(uint8_t ch, float angle) {
  if (ch > 15) return; // 安全检查

  // 记录逻辑角度
  servoAngle[ch] = angle;

  // 先加上校准偏移
  float realAngle = angle + servoOffset[ch];

  // 是否反向
  if (servoReversed[ch]) {
    realAngle = 180.0 - realAngle;
  }

  uint16_t pulse = angleToPulse(realAngle);
  pwm.setPWM(ch, 0, pulse);
}

// 批量设置多个舵机角度，带时间渐变（简单线性插值）
void moveServosSmooth(int duration_ms, float targetAngles[8]) {
  const int steps = 20;  // 越大越平滑，但越慢
  int stepDelay = duration_ms / steps;

  float startAngles[8];
  for (int i = 0; i < 8; i++) {
    startAngles[i] = servoAngle[i];
  }

  for (int s = 1; s <= steps; s++) {
    float alpha = (float)s / steps;
    for (int i = 0; i < 8; i++) {
      float a = startAngles[i] + (targetAngles[i] - startAngles[i]) * alpha;
      setServoAngle(i, a);
    }
    delay(stepDelay);
  }
}

// 让全部舵机回到"站立姿态"（中立角度，暂用 90°）
void standPose() {
  float neutral[8] = {90,90,90,90,90,90,90,90};
  moveServosSmooth(500, neutral);
}

// 简单前进动作（四步一个周期）
void forwardStep() {
  // 这里是非常简化的示范动作，你后面可以慢慢优化 gait

  // 基础中立角
  float base[8] = {90,90,90,90, 90,90,90,90};

  // 抬起并前摆：左前 & 右后
  float pose1[8] = {
    70,  // LF_HIP 前摆一点
    90,  // RF_HIP
    90,  // LR_HIP
    110, // RR_HIP 后摆一点

    60,  // LF_KNEE 抬腿
    90,  // RF_KNEE
    90,  // LR_KNEE
    60   // RR_KNEE 抬腿
  };
  moveServosSmooth(250, pose1);

  // 落下脚
  float pose2[8] = {
    70,90,90,110,
    90,90,90,90
  };
  moveServosSmooth(200, pose2);

  // 另一对：右前 & 左后
  float pose3[8] = {
    90, 110, 70,  90,
    90,  60, 60,  90
  };
  moveServosSmooth(250, pose3);

  float pose4[8] = {
    90,110,70,90,
    90,90,90,90
  };
  moveServosSmooth(200, pose4);

  // 回到中立（可选）
  moveServosSmooth(200, base);
}

// 简单后退：反过来摆
void backwardStep() {
  float base[8] = {90,90,90,90, 90,90,90,90};

  float pose1[8] = {
    110, // LF 后摆
    90,
    90,
    70,  // RR 前摆

    60,90,90,60
  };
  moveServosSmooth(250, pose1);

  float pose2[8] = {110,90,90,70, 90,90,90,90};
  moveServosSmooth(200, pose2);

  float pose3[8] = {
    90,
    70,  // RF 后摆
    110, // LR 前摆
    90,

    90,60,60,90
  };
  moveServosSmooth(250, pose3);

  float pose4[8] = {90,70,110,90, 90,90,90,90};
  moveServosSmooth(200, pose4);

  moveServosSmooth(200, base);
}

// 原地左转（前腿向左摆，后腿向右摆）
void turnLeftStep() {
  float base[8] = {90,90,90,90, 90,90,90,90};

  float pose1[8] = {
    70, 110, 110, 70,   // 髋：前左往前，前右往后，后左往后，后右往前
    70, 70,  70,  70    // 四条腿稍微抬一点
  };
  moveServosSmooth(300, pose1);
  moveServosSmooth(300, base);
}

// 原地右转
void turnRightStep() {
  float base[8] = {90,90,90,90, 90,90,90,90};

  float pose1[8] = {
    110, 70, 70, 110,
    70,  70, 70, 70
  };
  moveServosSmooth(300, pose1);
  moveServosSmooth(300, base);
}

// 串口帮助说明
void printHelp() {
  Serial.println(F("==== 蜘蛛机器人控制帮助 ===="));
  Serial.println(F("手动调整舵机角度："));
  Serial.println(F("  在串口输入: id angle"));
  Serial.println(F("  例如: 0 90   (把通道0调到90度)"));
  Serial.println(F(""));
  Serial.println(F("动作指令："));
  Serial.println(F("  f : 向前走一步"));
  Serial.println(F("  b : 向后退一步"));
  Serial.println(F("  l : 左转一步"));
  Serial.println(F("  r : 右转一步"));
  Serial.println(F("  h : 回到站立姿态(全部90度)"));
  Serial.println(F("  p : 打印当前 8 个通道的角度"));
  Serial.println(F("  ? : 显示本帮助"));
  Serial.println(F("============================"));
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println();
  Serial.println(F("ESP8266 + PCA9685 蜘蛛机器人启动"));

  // I2C 初始化（SDA, SCL）
  Wire.begin(D1, D2);  // 按你接线：SDA->D1, SCL->D2

  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  delay(10);

  // 初始化：先把目前逻辑角度当作 45°，然后缓慢移动到 90°
  for (int i = 0; i < 8; i++) {
    setServoAngle(i, servoAngle[i]);  // 先写入初始45°
  }
  delay(500);
  standPose();  // 全部移动到 90°
  printHelp();
}

void loop() {
  // 处理单字符动作命令
  if (Serial.available()) {
    // 先看看是不是一行"id angle"
    if (Serial.peek() >= '0' && Serial.peek() <= '9') {
      String line = Serial.readStringUntil('\n');
      line.trim();
      if (line.length() > 0) {
        int spaceIndex = line.indexOf(' ');
        if (spaceIndex > 0) {
          int id = line.substring(0, spaceIndex).toInt();
          int angle = line.substring(spaceIndex + 1).toInt();

          if (id >= 0 && id < 8) {
            Serial.print(F("设置通道 "));
            Serial.print(id);
            Serial.print(F(" 到 "));
            Serial.print(angle);
            Serial.println(F(" 度"));
            setServoAngle(id, angle);
          } else {
            Serial.println(F("通道 id 必须在 0~7"));
          }
        }
      }
    } else {
      char c = Serial.read();
      switch (c) {
        case 'f':
          Serial.println(F("向前一步"));
          forwardStep();
          break;
        case 'b':
          Serial.println(F("向后一步"));
          backwardStep();
          break;
        case 'l':
          Serial.println(F("左转一步"));
          turnLeftStep();
          break;
        case 'r':
          Serial.println(F("右转一步"));
          turnRightStep();
          break;
        case 'h':
          Serial.println(F("回到站立姿态"));
          standPose();
          break;
        case 'p':
          Serial.println(F("当前舵机角度："));
          for (int i = 0; i < 8; i++) {
            Serial.print(i);
            Serial.print(F(": "));
            Serial.print(servoAngle[i]);
            Serial.println(F(" deg"));
          }
          break;
        case '?':
          printHelp();
          break;
      }
    }
  }
}

使用建议（实际调机步骤）

  1. 先保证接线正确

    • PCA9685 供电要足够（舵机电源最好独立 5V，大电流，GND 与 ESP8266 共地）
    • SDA→D1, SCL→D2；其它电源线按板子说明连接。

  2. 上电 / 烧录后：

    • 打开串口监视器（115200 波特率，换行 \n）
    • 机器人会自动把 8 个舵机缓慢移动到 90°（站立姿态）

  3. 手动校准：

    • 例如要调通道 0 到 80°：在串口输入：0 80 回车
    • 慢慢调每个关节，让机器人站得"端正"
    • 把每个通道的最终"中立值"记下来，填进代码里的 servoOffset[] 或者改成 neutral[]。

  4. 测试动作：

    • 串口输入：f → 前进一步
    • 输入：b → 后退
    • 输入：l / r → 左转 / 右转
    • 随时输入 h 让它回到站立姿态

四足机器人 - ESP8266 + PCA9685 + 8舵机（带 trim 持久化）

SDA -> D2 (GPIO4), SCL -> D1 (GPIO5)

PCA9685 通道分配：

0: 左上（左前腿 - 髋/肩）

1: 右上（右前腿 - 鋒/肩）

2: 左下（左后腿 - 髋/肩）

3: 右下（右后腿 - 髋/肩）

4: 左上前（左前腿 - 膝/抬腿）

5: 右上前（右前腿 - 膝/抬腿）

6: 左下前（左后腿 - 膝/抬腿）

7: 右下前（右后腿 - 膝/抬腿）

from machine import Pin, I2C

import time

=============== PCA9685 驱动类 ===================

class PCA9685:

_MODE1 = 0x00

_PRESCALE = 0xFE

_LED0_ON_L = 0x06

复制代码 
def __init__(self, i2c, addr=0x40):
    self.i2c = i2c
    self.addr = addr
    self.reset()

def reset(self):
    # 复位寄存器
    self.i2c.writeto_mem(self.addr, self._MODE1, b'\x00')
    time.sleep_ms(5)

def set_pwm_freq(self, freq):
    # 设置 PWM 频率（舵机一般用 50Hz）
    prescaleval = int(25000000.0 / (4096 * freq) - 1)
    oldmode = self.i2c.readfrom_mem(self.addr, self._MODE1, 1)[0]
    newmode = (oldmode & 0x7F) | 0x10      # sleep
    self.i2c.writeto_mem(self.addr, self._MODE1, bytes([newmode]))
    self.i2c.writeto_mem(self.addr, self._PRESCALE, bytes([prescaleval]))
    self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode]))
    time.sleep_ms(5)
    self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode | 0xA1]))  # 重启 + 自动递增

def set_pwm(self, channel, on, off):
    # 设置指定通道的 on/off 计数（0~4095）
    data = bytearray(4)
    data[0] = on & 0xFF
    data[1] = (on >> 8) & 0xFF
    data[2] = off & 0xFF
    data[3] = (off >> 8) & 0xFF
    self.i2c.writeto_mem(self.addr, self._LED0_ON_L + 4 * channel, data)

=============== I2C 初始化（注意：D2=SDA, D1=SCL） ===================

実测结果：SDA = GPIO4(D2), SCL = GPIO5(D1)

try:

i2c = I2C(scl=Pin(5), sda=Pin(4)) # D1 = SCL, D2 = SDA

except TypeError:

i2c = I2C(-1, scl=Pin(5), sda=Pin(4))

pca = PCA9685(i2c, addr=0x40) # PCA9685 地址 0x40

pca.set_pwm_freq(50) # 舵机 50Hz

=============== 舵机控制封装 & trim 持久化 ===================

SERVO_MIN_US = 500 # 对应 0°

SERVO_MAX_US = 2500 # 对应 180°

PWM_PERIOD_US = 20000 # 50Hz -> 20ms

NEUTRAL_ANGLE = 90 # 理论中位

当前 8 个舵机"物理角度"记录（0~180，用于显示）

servo_angles = [NEUTRAL_ANGLE] * 8

舵机方向（某些安装反向，可设为 -1）

servo_dir = [1] * 8

每个舵机的 trim（相对 90° 的偏移，-90~+90），会保存到文件

TRIM_FILE = "servo_trim.txt"

servo_trim = [0] * 8 # 启动默认 0，之后从文件加载

def load_trims():

global servo_trim

try:

with open(TRIM_FILE) as f:

s = f.read().strip()

if not s:

print("trim 文件为空，使用默认 0 偏移。")

return

parts = s.split(',')

if len(parts) != 8:

print("trim 文件格式不正确，使用默认 0 偏移。")

return

vals = []

for p in parts:

v = int§

if v < -90: v = -90

if v > 90: v = 90

vals.append(v)

servo_trim = vals

print("已加载 trim 值:", servo_trim)

except Exception as e:

print("未找到 trim 文件，使用默认 0 偏移。")

def save_trims():

try:

with open(TRIM_FILE, "w") as f:

text = ",".join(str(int(max(-90, min(90, t)))) for t in servo_trim)

f.write(text)

print("trim 值已保存:", servo_trim)

except Exception as e:

print("保存 trim 失败:", e)

load_trims()

def angle_to_ticks(angle):

角度（0~180） -> PCA9685 的 0~4095 计数值

angle = max(0, min(180, angle))

pulse_us = SERVO_MIN_US + (SERVO_MAX_US - SERVO_MIN_US) * angle / 180.0

ticks = int(pulse_us * 4096 / PWM_PERIOD_US)

if ticks < 0: ticks = 0

if ticks > 4095: ticks = 4095

return ticks

def set_servo(channel, angle):

"""设置单个舵机物理角度（0~180°）"""

if servo_dir[channel] == -1:

angle = 180 - angle

ticks = angle_to_ticks(angle)

pca.set_pwm(channel, 0, ticks)

servo_angles[channel] = angle

def channel_neutral_angle(ch):

"""返回某通道的"中立物理角度" = 90° + trim，并限制在 0~180"""

base = NEUTRAL_ANGLE + servo_trim[ch]

if base < 0: base = 0

if base > 180: base = 180

return base

def all_to_neutral():

"""所有舵机转到各自的中立角度（含 trim）"""

for ch in range(8):

set_servo(ch, channel_neutral_angle(ch))

time.sleep_ms(20)

def all_to_angle(angle):

"""所有舵机都转到同一个物理角度（通常用不到了）"""

for ch in range(8):

set_servo(ch, angle)

time.sleep_ms(20)

=============== 机器人腿的语义映射 ===================

CH_LU = 0 # 左上（左前腿，髋/肩）

CH_RU = 1 # 右上

CH_LD = 2 # 左下（左后腿）

CH_RD = 3 # 右下

CH_LU_F = 4 # 左上前（左前腿膝/抬腿）

CH_RU_F = 5 # 右上前

CH_LD_F = 6 # 左下前

CH_RD_F = 7 # 右下前

def leg_neutral_angles(leg):

"""返回某条腿的 (hip_neutral, knee_neutral) 物理角度"""

if leg == 'LF':

return channel_neutral_angle(CH_LU), channel_neutral_angle(CH_LU_F)

if leg == 'RF':

return channel_neutral_angle(CH_RU), channel_neutral_angle(CH_RU_F)

if leg == 'LB':

return channel_neutral_angle(CH_LD), channel_neutral_angle(CH_LD_F)

if leg == 'RB':

return channel_neutral_angle(CH_RD), channel_neutral_angle(CH_RD_F)

return NEUTRAL_ANGLE, NEUTRAL_ANGLE

def leg_set(leg, hip_angle, knee_angle):

"""

leg: 'LF','RF','LB','RB'（左前、右前、左后、右后）

hip_angle / knee_angle：物理角度（0~180）

"""

if leg == 'LF':

set_servo(CH_LU, hip_angle)

set_servo(CH_LU_F, knee_angle)

elif leg == 'RF':

set_servo(CH_RU, hip_angle)

set_servo(CH_RU_F, knee_angle)

elif leg == 'LB':

set_servo(CH_LD, hip_angle)

set_servo(CH_LD_F, knee_angle)

elif leg == 'RB':

set_servo(CH_RD, hip_angle)

set_servo(CH_RD_F, knee_angle)

def neutral_pose():

"""站立中立姿态：所有舵机 = 90° + 各自 trim"""

all_to_neutral()

=============== 基本步态参数 ===================

HIP_SWING = 25 # 髋关节左右摆动幅度

KNEE_LIFT = 25 # 膝关节抬腿幅度

STEP_DELAY = 0.20 # 每小动作间的延时（秒）

=============== 步态动作（基于每个关节的 trim 后中位） ===================

def step_forward_once():

"""前进一步（简化 trot 步态）"""

复制代码 
# 先取各腿的中立角度
LF_hip0, LF_knee0 = leg_neutral_angles('LF')
RF_hip0, RF_knee0 = leg_neutral_angles('RF')
LB_hip0, LB_knee0 = leg_neutral_angles('LB')
RB_hip0, RB_knee0 = leg_neutral_angles('RB')

# 阶段 1：抬起 左前(LF) + 右后(RB)
leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 阶段 2：向前迈 LF，向后蹬 RB
leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 阶段 3：放下 LF、RB
leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)
leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
time.sleep(STEP_DELAY)

# 阶段 4：右前(RF) + 左后(LB) 调整
leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0)
leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0)
time.sleep(STEP_DELAY)

neutral_pose()
time.sleep(STEP_DELAY)

def step_backward_once():

"""后退一步"""

复制代码 
LF_hip0, LF_knee0 = leg_neutral_angles('LF')
RF_hip0, RF_knee0 = leg_neutral_angles('RF')
LB_hip0, LB_knee0 = leg_neutral_angles('LB')
RB_hip0, RB_knee0 = leg_neutral_angles('RB')

# 抬起 左前 + 右后
leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 向后迈 LF，向前蹬 RB
leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 放下
leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0)
leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0)
time.sleep(STEP_DELAY)

# RF + LB 调整
leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
time.sleep(STEP_DELAY)

neutral_pose()
time.sleep(STEP_DELAY)

def turn_left_once():

"""原地左转一下（右侧腿跨大步）"""

复制代码 
LF_hip0, LF_knee0 = leg_neutral_angles('LF')
RF_hip0, RF_knee0 = leg_neutral_angles('RF')
LB_hip0, LB_knee0 = leg_neutral_angles('LB')
RB_hip0, RB_knee0 = leg_neutral_angles('RB')

# 抬起右侧两条腿（RF + RB）
leg_set('RF', RF_hip0,         RF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 右侧向前迈/向后蹬
leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0 - KNEE_LIFT)
leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 落地
leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
time.sleep(STEP_DELAY)

neutral_pose()
time.sleep(STEP_DELAY)

def turn_right_once():

"""原地右转一下（左侧腿跨大步）"""

复制代码 
LF_hip0, LF_knee0 = leg_neutral_angles('LF')
RF_hip0, RF_knee0 = leg_neutral_angles('RF')
LB_hip0, LB_knee0 = leg_neutral_angles('LB')
RB_hip0, RB_knee0 = leg_neutral_angles('RB')

# 抬起左侧两条腿（LF + LB）
leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
leg_set('LB', LB_hip0,         LB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 左侧向前迈/向后蹬
leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)
leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0 - KNEE_LIFT)
time.sleep(STEP_DELAY)

# 落地
leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)
leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
time.sleep(STEP_DELAY)

neutral_pose()
time.sleep(STEP_DELAY)

=============== 手动调节菜单（设置 trim） ===================

def print_servo_map():

print("舵机通道对应：")

print(" 0: 左上 (LF 髋)")

print(" 1: 右上 (RF 髋)")

print(" 2: 左下 (LB 髋)")

print(" 3: 右下 (RB 髋)")

print(" 4: 左上前 (LF 膝)")

print(" 5: 右上前 (RF 膝)")

print(" 6: 左下前 (LB 膝)")

print(" 7: 右下前 (RB 膝)")

print()

def manual_adjust():

"""

手动调节某个舵机的 trim：

  • 输入的是相对 90° 的偏移 (-90~+90)

  • 会保存到文件，下次上电自动生效

"""

global servo_trim

print_servo_map()

while True:

try:

s = input("输入舵机通道(0-7)，或 q 返回主菜单: ").strip()

except EOFError:

return

if s.lower() == 'q':

return

if not s.isdigit():

print("请输入 0-7 或 q")

continue

ch = int(s)

if ch < 0 or ch > 7:

print("通道范围 0-7")

continue

复制代码 
    curr_trim = servo_trim[ch]
    curr_neutral = channel_neutral_angle(ch)
    print("当前通道 {}: trim = {:+d}°，中立物理角度 ≈ {:.1f}°".format(ch, curr_trim, curr_neutral))

    off_s = input("输入新的 trim (-90 ~ +90，相对于 90°): ").strip()
    try:
        off = int(off_s)
    except:
        print("trim 必须是数字")
        continue
    if off < -90 or off > 90:
        print("trim 范围 -90 ~ +90")
        continue

    servo_trim[ch] = off
    save_trims()

    new_neutral = channel_neutral_angle(ch)
    set_servo(ch, new_neutral)
    print("通道 {} 新 trim = {:+d}°，中立角度 ≈ {:.1f}°".format(ch, off, new_neutral))
    time.sleep(0.1)

=============== 主循环菜单 ===================

def main():

print("初始化：按 trim 让所有舵机转到中立位置...")

neutral_pose()

time.sleep(1)

print("初始化完成。")

复制代码 
while True:
    print("\n===== 蜘蛛机器人 控制菜单 =====")
    print(" m : 手动调节舵机 trim（-90~+90，永久保存）")
    print(" w : 前进一步")
    print(" s : 后退一步")
    print(" a : 左转一下")
    print(" d : 右转一下")
    print(" n : 回到中立站立姿态 (90° + trim)")
    print(" q : 退出（停止程序）")
    try:
        cmd = input("请输入命令: ").strip().lower()
    except EOFError:
        break

    if cmd == 'm':
        manual_adjust()
    elif cmd == 'w':
        step_forward_once()
    elif cmd == 's':
        step_backward_once()
    elif cmd == 'a':
        turn_left_once()
    elif cmd == 'd':
        turn_right_once()
    elif cmd == 'n':
        neutral_pose()
    elif cmd == 'q':
        print("退出主循环。")
        break
    else:
        print("未知命令。")

直接运行 main

if name == 'main ':

main()

bash 复制代码 
# 四足机器人 - ESP8266 + PCA9685 + 8舵机（带 trim 持久化）
# SDA -> D2 (GPIO4), SCL -> D1 (GPIO5)
# PCA9685 通道分配：
# 0: 左上（左前腿 - 髋/肩）
# 1: 右上（右前腿 - 鋒/肩）
# 2: 左下（左后腿 - 髋/肩）
# 3: 右下（右后腿 - 髋/肩）
# 4: 左上前（左前腿 - 膝/抬腿）
# 5: 右上前（右前腿 - 膝/抬腿）
# 6: 左下前（左后腿 - 膝/抬腿）
# 7: 右下前（右后腿 - 膝/抬腿）

from machine import Pin, I2C
import time

# =============== PCA9685 驱动类 ===================

class PCA9685:
    _MODE1      = 0x00
    _PRESCALE   = 0xFE
    _LED0_ON_L  = 0x06

    def __init__(self, i2c, addr=0x40):
        self.i2c = i2c
        self.addr = addr
        self.reset()

    def reset(self):
        # 复位寄存器
        self.i2c.writeto_mem(self.addr, self._MODE1, b'\x00')
        time.sleep_ms(5)

    def set_pwm_freq(self, freq):
        # 设置 PWM 频率（舵机一般用 50Hz）
        prescaleval = int(25000000.0 / (4096 * freq) - 1)
        oldmode = self.i2c.readfrom_mem(self.addr, self._MODE1, 1)[0]
        newmode = (oldmode & 0x7F) | 0x10      # sleep
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([newmode]))
        self.i2c.writeto_mem(self.addr, self._PRESCALE, bytes([prescaleval]))
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode]))
        time.sleep_ms(5)
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode | 0xA1]))  # 重启 + 自动递增

    def set_pwm(self, channel, on, off):
        # 设置指定通道的 on/off 计数（0~4095）
        data = bytearray(4)
        data[0] = on & 0xFF
        data[1] = (on >> 8) & 0xFF
        data[2] = off & 0xFF
        data[3] = (off >> 8) & 0xFF
        self.i2c.writeto_mem(self.addr, self._LED0_ON_L + 4 * channel, data)

# =============== I2C 初始化（注意：D2=SDA, D1=SCL） ===================

# 実测结果：SDA = GPIO4(D2), SCL = GPIO5(D1)
try:
    i2c = I2C(scl=Pin(5), sda=Pin(4))   # D1 = SCL, D2 = SDA
except TypeError:
    i2c = I2C(-1, scl=Pin(5), sda=Pin(4))

pca = PCA9685(i2c, addr=0x40)  # PCA9685 地址 0x40
pca.set_pwm_freq(50)           # 舵机 50Hz

# =============== 舵机控制封装 & trim 持久化 ===================

SERVO_MIN_US = 500      # 对应 0°
SERVO_MAX_US = 2500     # 对应 180°
PWM_PERIOD_US = 20000   # 50Hz -> 20ms

NEUTRAL_ANGLE = 90      # 理论中位

# 当前 8 个舵机"物理角度"记录（0~180，用于显示）
servo_angles = [NEUTRAL_ANGLE] * 8

# 舵机方向（某些安装反向，可设为 -1）
servo_dir = [1] * 8

# 每个舵机的 trim（相对 90° 的偏移，-90~+90），会保存到文件
TRIM_FILE = "servo_trim.txt"
servo_trim = [0] * 8   # 启动默认 0，之后从文件加载

def load_trims():
    global servo_trim
    try:
        with open(TRIM_FILE) as f:
            s = f.read().strip()
        if not s:
            print("trim 文件为空，使用默认 0 偏移。")
            return
        parts = s.split(',')
        if len(parts) != 8:
            print("trim 文件格式不正确，使用默认 0 偏移。")
            return
        vals = []
        for p in parts:
            v = int(p)
            if v < -90: v = -90
            if v > 90:  v = 90
            vals.append(v)
        servo_trim = vals
        print("已加载 trim 值:", servo_trim)
    except Exception as e:
        print("未找到 trim 文件，使用默认 0 偏移。")

def save_trims():
    try:
        with open(TRIM_FILE, "w") as f:
            text = ",".join(str(int(max(-90, min(90, t)))) for t in servo_trim)
            f.write(text)
        print("trim 值已保存:", servo_trim)
    except Exception as e:
        print("保存 trim 失败:", e)

load_trims()

def angle_to_ticks(angle):
    # 角度（0~180） -> PCA9685 的 0~4095 计数值
    angle = max(0, min(180, angle))
    pulse_us = SERVO_MIN_US + (SERVO_MAX_US - SERVO_MIN_US) * angle / 180.0
    ticks = int(pulse_us * 4096 / PWM_PERIOD_US)
    if ticks < 0: ticks = 0
    if ticks > 4095: ticks = 4095
    return ticks

def set_servo(channel, angle):
    """设置单个舵机物理角度（0~180°）"""
    if servo_dir[channel] == -1:
        angle = 180 - angle
    ticks = angle_to_ticks(angle)
    pca.set_pwm(channel, 0, ticks)
    servo_angles[channel] = angle

def channel_neutral_angle(ch):
    """返回某通道的"中立物理角度" = 90° + trim，并限制在 0~180"""
    base = NEUTRAL_ANGLE + servo_trim[ch]
    if base < 0: base = 0
    if base > 180: base = 180
    return base

def all_to_neutral():
    """所有舵机转到各自的中立角度（含 trim）"""
    for ch in range(8):
        set_servo(ch, channel_neutral_angle(ch))
        time.sleep_ms(20)

def all_to_angle(angle):
    """所有舵机都转到同一个物理角度（通常用不到了）"""
    for ch in range(8):
        set_servo(ch, angle)
        time.sleep_ms(20)

# =============== 机器人腿的语义映射 ===================

CH_LU  = 0  # 左上（左前腿，髋/肩）
CH_RU  = 1  # 右上
CH_LD  = 2  # 左下（左后腿）
CH_RD  = 3  # 右下

CH_LU_F = 4  # 左上前（左前腿膝/抬腿）
CH_RU_F = 5  # 右上前
CH_LD_F = 6  # 左下前
CH_RD_F = 7  # 右下前

def leg_neutral_angles(leg):
    """返回某条腿的 (hip_neutral, knee_neutral) 物理角度"""
    if leg == 'LF':
        return channel_neutral_angle(CH_LU),  channel_neutral_angle(CH_LU_F)
    if leg == 'RF':
        return channel_neutral_angle(CH_RU),  channel_neutral_angle(CH_RU_F)
    if leg == 'LB':
        return channel_neutral_angle(CH_LD),  channel_neutral_angle(CH_LD_F)
    if leg == 'RB':
        return channel_neutral_angle(CH_RD),  channel_neutral_angle(CH_RD_F)
    return NEUTRAL_ANGLE, NEUTRAL_ANGLE

def leg_set(leg, hip_angle, knee_angle):
    """
    leg: 'LF','RF','LB','RB'（左前、右前、左后、右后）
    hip_angle / knee_angle：物理角度（0~180）
    """
    if leg == 'LF':
        set_servo(CH_LU,  hip_angle)
        set_servo(CH_LU_F, knee_angle)
    elif leg == 'RF':
        set_servo(CH_RU,  hip_angle)
        set_servo(CH_RU_F, knee_angle)
    elif leg == 'LB':
        set_servo(CH_LD,  hip_angle)
        set_servo(CH_LD_F, knee_angle)
    elif leg == 'RB':
        set_servo(CH_RD,  hip_angle)
        set_servo(CH_RD_F, knee_angle)

def neutral_pose():
    """站立中立姿态：所有舵机 = 90° + 各自 trim"""
    all_to_neutral()

# =============== 基本步态参数 ===================

HIP_SWING = 25    # 髋关节左右摆动幅度
KNEE_LIFT = 25    # 膝关节抬腿幅度
STEP_DELAY = 0.20 # 每小动作间的延时（秒）

# =============== 步态动作（基于每个关节的 trim 后中位） ===================

def step_forward_once():
    """前进一步（简化 trot 步态）"""

    # 先取各腿的中立角度
    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 阶段 1：抬起 左前(LF) + 右后(RB)
    leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 阶段 2：向前迈 LF，向后蹬 RB
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 阶段 3：放下 LF、RB
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # 阶段 4：右前(RF) + 左后(LB) 调整
    leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0)
    leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

def step_backward_once():
    """后退一步"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 抬起 左前 + 右后
    leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 向后迈 LF，向前蹬 RB
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 放下
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # RF + LB 调整
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

def turn_left_once():
    """原地左转一下（右侧腿跨大步）"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 抬起右侧两条腿（RF + RB）
    leg_set('RF', RF_hip0,         RF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0,         RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 右侧向前迈/向后蹬
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0 - KNEE_LIFT)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 落地
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

def turn_right_once():
    """原地右转一下（左侧腿跨大步）"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 抬起左侧两条腿（LF + LB）
    leg_set('LF', LF_hip0,         LF_knee0 - KNEE_LIFT)
    leg_set('LB', LB_hip0,         LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 左侧向前迈/向后蹬
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)

    # 落地
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

# =============== 手动调节菜单（设置 trim） ===================

def print_servo_map():
    print("舵机通道对应：")
    print(" 0: 左上   (LF 髋)")
    print(" 1: 右上   (RF 髋)")
    print(" 2: 左下   (LB 髋)")
    print(" 3: 右下   (RB 髋)")
    print(" 4: 左上前 (LF 膝)")
    print(" 5: 右上前 (RF 膝)")
    print(" 6: 左下前 (LB 膝)")
    print(" 7: 右下前 (RB 膝)")
    print()

def manual_adjust():
    """
    手动调节某个舵机的 trim：
    - 输入的是相对 90° 的偏移 (-90~+90)
    - 会保存到文件，下次上电自动生效
    """
    global servo_trim
    print_servo_map()
    while True:
        try:
            s = input("输入舵机通道(0-7)，或 q 返回主菜单: ").strip()
        except EOFError:
            return
        if s.lower() == 'q':
            return
        if not s.isdigit():
            print("请输入 0-7 或 q")
            continue
        ch = int(s)
        if ch < 0 or ch > 7:
            print("通道范围 0-7")
            continue

        curr_trim = servo_trim[ch]
        curr_neutral = channel_neutral_angle(ch)
        print("当前通道 {}: trim = {:+d}°，中立物理角度 ≈ {:.1f}°".format(ch, curr_trim, curr_neutral))

        off_s = input("输入新的 trim (-90 ~ +90，相对于 90°): ").strip()
        try:
            off = int(off_s)
        except:
            print("trim 必须是数字")
            continue
        if off < -90 or off > 90:
            print("trim 范围 -90 ~ +90")
            continue

        servo_trim[ch] = off
        save_trims()

        new_neutral = channel_neutral_angle(ch)
        set_servo(ch, new_neutral)
        print("通道 {} 新 trim = {:+d}°，中立角度 ≈ {:.1f}°".format(ch, off, new_neutral))
        time.sleep(0.1)

# =============== 主循环菜单 ===================

def main():
    print("初始化：按 trim 让所有舵机转到中立位置...")
    neutral_pose()
    time.sleep(1)
    print("初始化完成。")

    while True:
        print("\n===== 蜘蛛机器人 控制菜单 =====")
        print(" m : 手动调节舵机 trim（-90~+90，永久保存）")
        print(" w : 前进一步")
        print(" s : 后退一步")
        print(" a : 左转一下")
        print(" d : 右转一下")
        print(" n : 回到中立站立姿态 (90° + trim)")
        print(" q : 退出（停止程序）")
        try:
            cmd = input("请输入命令: ").strip().lower()
        except EOFError:
            break

        if cmd == 'm':
            manual_adjust()
        elif cmd == 'w':
            step_forward_once()
        elif cmd == 's':
            step_backward_once()
        elif cmd == 'a':
            turn_left_once()
        elif cmd == 'd':
            turn_right_once()
        elif cmd == 'n':
            neutral_pose()
        elif cmd == 'q':
            print("退出主循环。")
            break
        else:
            print("未知命令。")

# 直接运行 main
if __name__ == '__main__':
    main()
bash 复制代码 
# 四足机器人 - ESP8266 + PCA9685 + 8舵机（带 trim 持久化 + 网页控制）
# WiFi: SSID = jerry, 密码 = ldk123456
# SDA -> D2 (GPIO4), SCL -> D1 (GPIO5)
#
# PCA9685 通道分配：
# 0: 左上（左前腿 - 髋/肩）
# 1: 右上（右前腿 - 髋/肩）
# 2: 左下（左后腿 - 髋/肩）
# 3: 右下（右后腿 - 髋/肩）
# 4: 左上前（左前腿 - 膝/抬腿）
# 5: 右上前（右前腿 - 膝/抬腿）
# 6: 左下前（左后腿 - 膝/抬腿）
# 7: 右下前（右后腿 - 膝/抬腿）

from machine import Pin, I2C
import time
import network
import socket

# =============== PCA9685 驱动类 ===================

class PCA9685:
    _MODE1      = 0x00
    _PRESCALE   = 0xFE
    _LED0_ON_L  = 0x06

    def __init__(self, i2c, addr=0x40):
        self.i2c = i2c
        self.addr = addr
        self.reset()

    def reset(self):
        # 复位寄存器
        self.i2c.writeto_mem(self.addr, self._MODE1, b'\x00')
        time.sleep_ms(5)

    def set_pwm_freq(self, freq):
        # 设置 PWM 频率（舵机一般用 50Hz）
        prescaleval = int(25000000.0 / (4096 * freq) - 1)
        oldmode = self.i2c.readfrom_mem(self.addr, self._MODE1, 1)[0]
        newmode = (oldmode & 0x7F) | 0x10      # sleep
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([newmode]))
        self.i2c.writeto_mem(self.addr, self._PRESCALE, bytes([prescaleval]))
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode]))
        time.sleep_ms(5)
        self.i2c.writeto_mem(self.addr, self._MODE1, bytes([oldmode | 0xA1]))  # 重启 + 自动递增

    def set_pwm(self, channel, on, off):
        # 设置指定通道的 on/off 计数（0~4095）
        data = bytearray(4)
        data[0] = on & 0xFF
        data[1] = (on >> 8) & 0xFF
        data[2] = off & 0xFF
        data[3] = (off >> 8) & 0xFF
        self.i2c.writeto_mem(self.addr, self._LED0_ON_L + 4 * channel, data)

# =============== I2C 初始化（D2=SDA, D1=SCL） ===================

try:
    i2c = I2C(scl=Pin(5), sda=Pin(4))   # D1 = SCL, D2 = SDA
except TypeError:
    i2c = I2C(-1, scl=Pin(5), sda=Pin(4))

pca = PCA9685(i2c, addr=0x40)  # PCA9685 地址 0x40
pca.set_pwm_freq(50)           # 舵机 50Hz

# =============== 舵机控制 & trim 持久化 ===================

SERVO_MIN_US = 500      # 对应 0°
SERVO_MAX_US = 2500     # 对应 180°
PWM_PERIOD_US = 20000   # 50Hz -> 20ms

NEUTRAL_ANGLE = 90      # 理论中位

# 当前 8 个舵机"物理角度"记录（0~180，用于显示）
servo_angles = [NEUTRAL_ANGLE] * 8

# 舵机方向（某些安装反向，可设为 -1）
servo_dir = [1] * 8

# 每个舵机的 trim（相对 90° 的偏移，-90~+90），会保存到文件
TRIM_FILE = "servo_trim.txt"
servo_trim = [0] * 8   # 默认 0，之后从文件加载

def load_trims():
    global servo_trim
    try:
        with open(TRIM_FILE) as f:
            s = f.read().strip()
        if not s:
            print("trim 文件为空，使用默认 0 偏移。")
            return
        parts = s.split(',')
        if len(parts) != 8:
            print("trim 文件格式不正确，使用默认 0 偏移。")
            return
        vals = []
        for p in parts:
            v = int(p)
            if v < -90: v = -90
            if v > 90:  v = 90
            vals.append(v)
        servo_trim = vals
        print("已加载 trim 值:", servo_trim)
    except Exception as e:
        print("未找到 trim 文件，使用默认 0 偏移。")

def save_trims():
    try:
        with open(TRIM_FILE, "w") as f:
            text = ",".join(str(int(max(-90, min(90, t)))) for t in servo_trim)
            f.write(text)
        print("trim 值已保存:", servo_trim)
    except Exception as e:
        print("保存 trim 失败:", e)

load_trims()

def angle_to_ticks(angle):
    # 角度（0~180） -> PCA9685 的 0~4095 计数值
    angle = max(0, min(180, angle))
    pulse_us = SERVO_MIN_US + (SERVO_MAX_US - SERVO_MIN_US) * angle / 180.0
    ticks = int(pulse_us * 4096 / PWM_PERIOD_US)
    if ticks < 0: ticks = 0
    if ticks > 4095: ticks = 4095
    return ticks

def set_servo(channel, angle):
    """设置单个舵机物理角度（0~180°）"""
    if servo_dir[channel] == -1:
        angle = 180 - angle
    ticks = angle_to_ticks(angle)
    pca.set_pwm(channel, 0, ticks)
    servo_angles[channel] = angle

def channel_neutral_angle(ch):
    """返回某通道的"中立物理角度" = 90° + trim，并限制在 0~180"""
    base = NEUTRAL_ANGLE + servo_trim[ch]
    if base < 0: base = 0
    if base > 180: base = 180
    return base

def all_to_neutral():
    """所有舵机转到各自的中立角度（含 trim）"""
    for ch in range(8):
        set_servo(ch, channel_neutral_angle(ch))
        time.sleep_ms(20)

def all_to_angle(angle):
    """所有舵机都转到同一个物理角度"""
    for ch in range(8):
        set_servo(ch, angle)
        time.sleep_ms(20)

# =============== 机器人腿的语义映射 ===================

CH_LU  = 0  # 左上（左前腿，髋/肩）
CH_RU  = 1  # 右上
CH_LD  = 2  # 左下（左后腿）
CH_RD  = 3  # 右下

CH_LU_F = 4  # 左上前（左前腿膝/抬腿）
CH_RU_F = 5  # 右上前
CH_LD_F = 6  # 左下前（左后腿膝）
CH_RD_F = 7  # 右下前（右后腿膝）

def leg_neutral_angles(leg):
    """返回某条腿的 (hip_neutral, knee_neutral) 物理角度"""
    if leg == 'LF':
        return channel_neutral_angle(CH_LU),  channel_neutral_angle(CH_LU_F)
    if leg == 'RF':
        return channel_neutral_angle(CH_RU),  channel_neutral_angle(CH_RU_F)
    if leg == 'LB':
        return channel_neutral_angle(CH_LD),  channel_neutral_angle(CH_LD_F)
    if leg == 'RB':
        return channel_neutral_angle(CH_RD),  channel_neutral_angle(CH_RD_F)
    return NEUTRAL_ANGLE, NEUTRAL_ANGLE

def leg_set(leg, hip_angle, knee_angle):
    """
    leg: 'LF','RF','LB','RB'（左前、右前、左后、右后）
    hip_angle / knee_angle：物理角度（0~180）
    """
    if leg == 'LF':
        set_servo(CH_LU,  hip_angle)
        set_servo(CH_LU_F, knee_angle)
    elif leg == 'RF':
        set_servo(CH_RU,  hip_angle)
        set_servo(CH_RU_F, knee_angle)
    elif leg == 'LB':
        set_servo(CH_LD,  hip_angle)
        set_servo(CH_LD_F, knee_angle)
    elif leg == 'RB':
        set_servo(CH_RD,  hip_angle)
        set_servo(CH_RD_F, knee_angle)

def neutral_pose():
    """站立中立姿态：所有舵机 = 90° + 各自 trim"""
    all_to_neutral()

# =============== 步态参数（更适合仿真慢步） ===================

HIP_SWING = 20    # 髋关节摆动幅度，适当减小
KNEE_LIFT = 18    # 膝关节抬腿幅度
STEP_DELAY = 0.15 # 每小动作间的延时

# =============== 爬行步态（一次只动一条腿） ===================

def step_forward_once():
    """
    爬行步态前进：
    顺序：左前 -> 右后 -> 右前 -> 左后
    始终保持 3 条腿着地，更稳定，更像慢速四足。
    """

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 定义"向前"的 hip 偏移策略：
    # 左腿：+HIP_SWING 为向前；右腿：-HIP_SWING 为向前
    # 这个方向如与实际相反，可以把对应腿的 servo_dir 改成 -1 调整

    # 1. 左前 (LF) 抬腿 -> 向前迈 -> 落地
    leg_set('LF', LF_hip0, LF_knee0 - KNEE_LIFT)                          # 抬腿
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)              # 向前
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)                          # 落地
    time.sleep(STEP_DELAY)

    # 2. 右后 (RB)
    leg_set('RB', RB_hip0, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # 3. 右前 (RF)
    leg_set('RF', RF_hip0, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0)
    time.sleep(STEP_DELAY)

    # 4. 左后 (LB)
    leg_set('LB', LB_hip0, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    # 收回到中立（可注释掉，看"连续姿态"效果）
    neutral_pose()
    time.sleep(STEP_DELAY)

def step_backward_once():
    """爬行步态后退（前进反向）"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 左前 (LF)：向后 = -HIP_SWING
    leg_set('LF', LF_hip0, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0)
    time.sleep(STEP_DELAY)

    # 右后 (RB)：向后 = +HIP_SWING
    leg_set('RB', RB_hip0, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # 右前 (RF)：向后 = +HIP_SWING
    leg_set('RF', RF_hip0, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
    time.sleep(STEP_DELAY)

    # 左后 (LB)：向后 = -HIP_SWING
    leg_set('LB', LB_hip0, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

def turn_left_once():
    """原地左转（右侧腿跨大步，左侧补偿）"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 思路：右侧腿向前，左侧腿向后，形成左转
    # 1. 右前 (RF)
    leg_set('RF', RF_hip0, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0 - KNEE_LIFT)  # 右前向前（假设 - 为前）
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 - HIP_SWING, RF_knee0)
    time.sleep(STEP_DELAY)

    # 2. 左后 (LB) 向后
    leg_set('LB', LB_hip0, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0 - KNEE_LIFT)  # 向后
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 - HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    # 3. 右后 (RB) 向前
    leg_set('RB', RB_hip0, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 - HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # 4. 左前 (LF) 向后
    leg_set('LF', LF_hip0, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 - HIP_SWING, LF_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

def turn_right_once():
    """原地右转（左侧腿跨大步，右侧补偿）"""

    LF_hip0, LF_knee0 = leg_neutral_angles('LF')
    RF_hip0, RF_knee0 = leg_neutral_angles('RF')
    LB_hip0, LB_knee0 = leg_neutral_angles('LB')
    RB_hip0, RB_knee0 = leg_neutral_angles('RB')

    # 1. 左前 (LF) 向前
    leg_set('LF', LF_hip0, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LF', LF_hip0 + HIP_SWING, LF_knee0)
    time.sleep(STEP_DELAY)

    # 2. 右后 (RB) 向后
    leg_set('RB', RB_hip0, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RB', RB_hip0 + HIP_SWING, RB_knee0)
    time.sleep(STEP_DELAY)

    # 3. 左后 (LB) 向前
    leg_set('LB', LB_hip0, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('LB', LB_hip0 + HIP_SWING, LB_knee0)
    time.sleep(STEP_DELAY)

    # 4. 右前 (RF) 向后
    leg_set('RF', RF_hip0, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0 - KNEE_LIFT)
    time.sleep(STEP_DELAY)
    leg_set('RF', RF_hip0 + HIP_SWING, RF_knee0)
    time.sleep(STEP_DELAY)

    neutral_pose()
    time.sleep(STEP_DELAY)

# =============== 手动 trim 调节（串口用） ===================

def print_servo_map():
    print("舵机通道对应：")
    print(" 0: 左上   (LF 髋)")
    print(" 1: 右上   (RF 髋)")
    print(" 2: 左下   (LB 髋)")
    print(" 3: 右下   (RB 髋)")
    print(" 4: 左上前 (LF 膝)")
    print(" 5: 右上前 (RF 膝)")
    print(" 6: 左下前 (LB 膝)")
    print(" 7: 右下前 (RB 膝)")
    print()

def manual_adjust():
    """
    串口下手动调节某个舵机的 trim：
    - 输入的是相对 90° 的偏移 (-90~+90)
    - 会保存到文件，下次上电自动生效
    """
    global servo_trim
    print_servo_map()
    while True:
        try:
            s = input("输入舵机通道(0-7)，或 q 返回主菜单: ").strip()
        except EOFError:
            return
        if s.lower() == 'q':
            return
        if not s.isdigit():
            print("请输入 0-7 或 q")
            continue
        ch = int(s)
        if ch < 0 or ch > 7:
            print("通道范围 0-7")
            continue

        curr_trim = servo_trim[ch]
        curr_neutral = channel_neutral_angle(ch)
        print("当前通道 {}: trim = {:+d}°，中立物理角度 ≈ {:.1f}°".format(ch, curr_trim, curr_neutral))

        off_s = input("输入新的 trim (-90 ~ +90，相对于 90°): ").strip()
        try:
            off = int(off_s)
        except:
            print("trim 必须是数字")
            continue
        if off < -90 or off > 90:
            print("trim 范围 -90 ~ +90")
            continue

        servo_trim[ch] = off
        save_trims()

        new_neutral = channel_neutral_angle(ch)
        set_servo(ch, new_neutral)
        print("通道 {} 新 trim = {:+d}°，中立角度 ≈ {:.1f}°".format(ch, off, new_neutral))
        time.sleep(0.1)

# =============== WiFi + 网页控制 ===================

SSID = "jerry"
PASSWORD = "ldk123456"

def wifi_connect():
    sta = network.WLAN(network.STA_IF)
    sta.active(True)
    if not sta.isconnected():
        print("连接 WiFi 中:", SSID)
        sta.connect(SSID, PASSWORD)
        # 等几秒钟
        for _ in range(20):
            if sta.isconnected():
                break
            time.sleep(0.5)
    if sta.isconnected():
        print("WiFi 已连接:", sta.ifconfig())
    else:
        print("WiFi 连接失败，仍可用串口控制。")
    return sta

def web_page():
    html = """<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8">
<title>四足机器人控制</title>
<style>
body { font-family: Arial; text-align: center; }
h1 { margin-top: 20px; }
.btn {
  display: inline-block;
  padding: 15px 30px;
  margin: 8px;
  font-size: 18px;
  text-decoration: none;
  border-radius: 8px;
  border: 1px solid #333;
}
.btn-main { }
.btn-small { font-size: 14px; padding: 8px 16px; }
</style>
</head>
<body>
<h1>四足机器人控制面板</h1>
<p>点击按钮让机器人动作（每次一步）</p>
<div>
  <a class="btn btn-small" href="/?cmd=n">回中立姿态</a>
</div>
<div>
  <a class="btn" href="/?cmd=w">⬆ 前进</a>
</div>
<div>
  <a class="btn" href="/?cmd=a">⬅ 左转</a>
  <a class="btn" href="/?cmd=d">右转 ➡</a>
</div>
<div>
  <a class="btn" href="/?cmd=s">⬇ 后退</a>
</div>
<p style="margin-top:30px;font-size:13px;color:#666;">
可以多次点击前进/后退/转向按钮来连续移动。<br/>
trim 校准仍建议通过串口菜单完成。
</p>
</body>
</html>
"""
    return html

def web_server():
    sta = wifi_connect()
    # 获取 IP
    if sta.isconnected():
        ip = sta.ifconfig()[0]
        print("在浏览器中打开: http://{}/".format(ip))
    else:
        print("WiFi 未连接，网页控制不可用。")

    addr = socket.getaddrinfo('0.0.0.0', 80)[0][-1]
    s = socket.socket()
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.bind(addr)
    s.listen(1)
    print("HTTP 服务器已启动，监听端口 80")

    while True:
        try:
            cl, addr = s.accept()
        except OSError:
            continue
        # print('客户端连接自', addr)
        req = cl.recv(1024)
        req = str(req)

        # 简单解析命令
        cmd = None
        if "/?cmd=w" in req:
            cmd = "w"
        elif "/?cmd=s" in req:
            cmd = "s"
        elif "/?cmd=a" in req:
            cmd = "a"
        elif "/?cmd=d" in req:
            cmd = "d"
        elif "/?cmd=n" in req:
            cmd = "n"

        if cmd == "w":
            step_forward_once()
        elif cmd == "s":
            step_backward_once()
        elif cmd == "a":
            turn_left_once()
        elif cmd == "d":
            turn_right_once()
        elif cmd == "n":
            neutral_pose()

        response = web_page()
        cl.send('HTTP/1.1 200 OK\r\nContent-Type: text/html\r\nConnection: close\r\n\r\n')
        cl.send(response)
        cl.close()

# =============== 主程序 ===================

def main():
    print("初始化：按 trim 让所有舵机转到中立位置...")
    neutral_pose()
    time.sleep(1)
    print("初始化完成。启动 WiFi + 网页控制。")
    web_server()   # 进入 HTTP 循环

# 如果需要串口菜单，可自己在 REPL 中调用 manual_adjust()

if __name__ == '__main__':
    main()
相关推荐
honeysuckle_luo1 小时前
香橙派ai pro安装支持昇腾NPU的ollama
linux·运维·服务器
通往曙光的路上1 小时前
WebSocket
网络·websocket·网络协议
Cher ~1 小时前
【路由器】路由表
网络·智能路由器
池央2 小时前
从“算子不支持”到“NPU高效执行”：CANN 8.0 TBE 自定义算子落地实践
linux·人工智能
_西瓜_2 小时前
Google Antigravity 登录失败？中国地区完整解决方案与排查指南
网络
小锋学长生活大爆炸2 小时前
【教程】CentOS在不知道root密码的情况下进入shell和重置密码
linux·运维·centos
weixin_46682 小时前
Docker常用命令与操作
运维·docker·容器
代码游侠2 小时前
学习笔记——GDB调试工具
linux·开发语言·笔记·学习
wanhengidc2 小时前
云手机 多样化的云服务产品
运维·服务器·科技·游戏·智能手机
热门推荐
01GitHub 镜像站点02【超详细教程】手把手教你从微软官网免费下载Windows 10官方原版ISO镜像（2025最新版）03UV安装并设置国内源04BongoCat - 跨平台键盘猫动画工具05安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）06本地部署阿里最新开源的Z-Image07Linux下V2Ray安装配置指南08Labelme从安装到标注：零基础完整指南09【保姆级教程】免费使用Gemini3的5种方法！免翻墙/国内直连10Meta第三代“分割一切”模型——SAM 3本地部署教程：首支持文本提示分割，400万概念、30毫秒响应，检测分割追踪一网打尽