机器人控制程序

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()
相关推荐
宴之敖者、17 小时前
Linux——\r,\n和缓冲区
linux·运维·服务器
LuDvei17 小时前
LINUX错误提示函数
linux·运维·服务器
未来可期LJ17 小时前
【Linux 系统】进程间的通信方式
linux·服务器
Abona17 小时前
C语言嵌入式全栈Demo
linux·c语言·面试
心理之旅18 小时前
高校文献检索系统
运维·服务器·容器
Lenyiin18 小时前
Linux 基础IO
java·linux·服务器
The Chosen One98518 小时前
【Linux】深入理解Linux进程（一）：PCB结构、Fork创建与状态切换详解
linux·运维·服务器
Hill_HUIL18 小时前
学习日志22-静态路由
网络·学习
大佐不会说日语～18 小时前
使用Docker Compose 部署时网络冲突问题排查与解决
运维·网络·spring boot·docker·容器
Kira Skyler18 小时前
eBPF debugfs中的追踪点format实现原理
linux
热门推荐
01GitHub 镜像站点02OpenCode 入门教程：介绍 · 安装 · 配置第三方 API (如 Claude)03Claude Code Skills 实用使用手册042025 年大语言模型发展回顾：关键突破、意外转折与 2026 年展望05Clawdbot 中文汉化版 接入微信、飞书06一文了解国产算子编程语言 TileLang，TileLang 对国产开源生态的影响与启示07Vue-skills的中文文档08在Trae中使用Pencil MCP09Claude Code + GLM4.7 避坑指南：解决 Unable to connect to Anthropic services10UV安装并设置国内源