柳州门户网站建设公司排名学程序员大概要多少钱

柳州门户网站建设公司排名,学程序员大概要多少钱,做网站空间500m多少钱,网站建设费钱吗文章目录初步的技术路线图#xff1a;1. 舵机安装时的初始化2. 组装完机器人后的舵机微调3. 网页端控制机器人前进并返回技术实现要点#xff1a;系统架构图#xff1a;初始代码讲解一、整体结构是干啥的#xff1f;二、舵机驱动层1. 角度 → 脉宽2. ServoDriver3. 舵机引脚…文章目录初步的技术路线图1. 舵机安装时的初始化2. 组装完机器人后的舵机微调3. 网页端控制机器人前进并返回技术实现要点系统架构图初始代码讲解一、整体结构是干啥的二、舵机驱动层1. 角度 → 脉宽2. ServoDriver3. 舵机引脚 / 初始化三、几何参数与状态状态向量舵机 offset四、set_site / 插值 / 等待到位1. set_site2. servo_service_once3. wait_reach / wait_all_reach五、逆运动学 舵机映射1. cartesian_to_polar2. polar_to_servo重点六、修左后腿“末关节向里”的简单改法原来的 leg 3 映射建议改成让它的末关节方向跟 leg 1、leg 2 一致七、后续微调用 offset 来“磨平”使用说明先看这个校准程序 quadruped_calibrate.py接下来怎么用这些 offset1. 先把 4 条腿认清楚按你这份代码2. 当前 step_forward 实际谁先动3. 你现在这个 step_backward 的问题4. 正确的思路“交换前后腿角色 方向反过来”完全照着 step_forward 走5. 一版改好的 step_backward完整替换你现在的那个6. 预期效果这个四足蜘蛛型机器人的开发分为三个主要部分涉及硬件和软件的协调。初步的技术路线图1. 舵机安装时的初始化硬件使用ESP32控制板连接12个舵机每个舵机控制一个机器人关节确保舵机的电源和信号线正确连接。使用PWM控制信号驱动舵机。ESP32支持PWM输出可以通过MicroPython库来配置。软件在MicroPython中编写舵机初始化代码确保所有舵机的角度能够设置为初始值例如全部归零角度并能够进行简单的自检。通过WebSocket或HTTP与PHP网页进行通信让用户能够通过网页界面启动舵机初始化过程。2. 组装完机器人后的舵机微调硬件每个舵机的安装角度可能需要微调以确保机器人能够稳健地行走。软件在MicroPython中实现舵机微调功能可以通过输入舵机的目标角度来调整每个舵机的位置。提供一个可通过网页界面调节的UI让用户能实时修改每个舵机的角度。PHP网页通过API调用MicroPython代码发送舵机的角度控制指令到ESP32。3. 网页端控制机器人前进并返回硬件需要设计前进和返回的动作模式主要依赖于舵机角度的变化。软件在MicroPython中编写机器人行走的控制逻辑。定义机器人的步态和动作例如每个步长的舵机角度变化如何实现前进、后退等基本动作。使用PHP开发一个控制面板允许用户输入机器人前进的步数后端通过PHP将前进指令发送到MicroPython程序控制机器人按指令移动。在机器人完成前进动作后通过同步的舵机指令让机器人原路返回。技术实现要点MicroPython用于控制ESP32处理舵机控制、运动规划、传感器读取等。PHP用于开发网页控制界面提供与用户交互的方式将用户输入的命令通过API发送给ESP32。WebSocket / HTTP用于在PHP网页与ESP32之间进行实时通信控制机器人动作。系统架构图PHP网页前端用户输入控制指令如前进多少步并实时查看机器人的状态。PHP后端接收用户指令通过HTTP或WebSocket与ESP32进行通信。ESP32与MicroPython接收控制指令驱动舵机进行运动并通过WebSocket反馈状态。初始代码讲解一、整体结构是干啥的这个脚本干的事其实就三步驱动 12 个舵机用PWM输出 50Hz 方波按照角度算占空比。做逆运动学给出足端的 (x, y, z)算出三关节角度 (alpha, beta, gamma)再转换成舵机角度。实现一个对角步态按指定步数step_forward(steps)走路。模块划分上大致是舵机驱动层机器人几何/状态运动学 插值步态控制main()初始化 → 站立 → 前进 N 步二、舵机驱动层SERVO_MIN_US500SERVO_MAX_US2500SERVO_FREQ50PWM_MAX_DUTY65535这些就是舵机脉宽范围 500us ~ 2500us对应 Cattach(500, 2500)频率 50Hz周期 20msduty_u16满刻度 655351. 角度 → 脉宽defangle_to_us(angle):ifangle0:angle0elifangle180:angle180usSERVO_MIN_USangle*(SERVO_MAX_US-SERVO_MIN_US)/180.0...returnus逻辑0° → 500us180° → 2500us线性插值。2. ServoDriverclassServoDriver:def__init__(self,pin_num,freqSERVO_FREQ):self.pwmPWM(Pin(pin_num),freqfreq)self.pwm.duty_u16(0)defwrite_angle(self,angle):usangle_to_us(angle)period_us1_000_000//SERVO_FREQ# 20_000 usdutyint(us/period_us*PWM_MAX_DUTY)self.pwm.duty_u16(duty)write_angle(angle)把角度转成us再转成在 20ms 周期里的占空比用duty_u16写进去3. 舵机引脚 / 初始化# leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0[4,5,15],# leg 1[33,25,32],# leg 2[12,14,13],# leg 3]...definit_servos():forleginrange(4):forjinrange(3):servos[leg][j]ServoDriver(servo_pin[leg][j])servos[leg][joint]就是一只舵机对象。servo_write(leg, joint, angle)一层封装之后后面都不用关心引脚了。三、几何参数与状态length_a55.0# 大腿length_b77.5# 小腿length_c27.5# 髋关节前向偏移length_side71.0...z_default-50.0z_up-30.0z_bootz_absolute x_default62.0y_start0.0y_step40.0这些就是你的腿长和默认站姿参数和 C 代码保持一致。状态向量site_now[[0.0,0.0,0.0]for_inrange(4)]# 当前足端坐标site_expect[[0.0,0.0,0.0]for_inrange(4)]# 目标足端坐标temp_speed[[0.0,0.0,0.0]for_inrange(4)]# 每次迭代的增量你所有的动作其实都是在改site_expect然后通过插值让site_now慢慢追上site_expect。舵机 offsetoffset[[0.0,0.0,0.0],# leg 0[0.0,0.0,0.0],# leg 1[0.0,0.0,0.0],# leg 2[0.0,0.0,0.0],# leg 3]这是用来做“舵机中位校准”的装完后发现某个舵机偏了 5°就可以在这里加/减几度做微调。注意这个是“小修正”不适合用来修“方向完全反了”的问题那要改映射公式。四、set_site / 插值 / 等待到位1. set_sitedefset_site(leg,x,y,z):...# x/y/z 与当前 site_now 的差值length_xx-site_now[leg][0]# 如果 x ! KEEP...lengthsqrt(length_x^2length_y^2length_z^2)iflengthEPS:temp_speed[...]0else:temp_speed[leg][axis]length_axis/length*move_speed*speed_multiple计算目标点与当前点的距离向量(length_x, length_y, length_z)按照当前的move_speed算出单位方向上的速度向量储存在temp_speed[leg]里KEEP表示这个轴不动用当前值2. servo_service_oncedefservo_service_once():forleginrange(4):# 1. site_now 往 site_expect 走一步...# 2. 把新的 site_now 做逆运动学alpha,beta,gammacartesian_to_polar(...)# 3. 映射为舵机角度写 PWMpolar_to_servo(leg,alpha,beta,gamma)这是整个系统的“心跳函数”每次被调用所有腿都往目标方向移动一点点然后更新舵机角。3. wait_reach / wait_all_reachdefwait_reach(leg):whilenotleg_reached(leg):servo_service_once()time.sleep_ms(10)就是循环调用“心跳”直到某条腿全部坐标都达到目标误差 EPS。五、逆运动学 舵机映射1. cartesian_to_polardefcartesian_to_polar(x,y,z):w(1ifx0else-1)*sqrt(x*xy*y)vw-length_c...alphaatan2(z,v)acos(...)betaacos(...)ifw0:gammaatan2(y,x)else:gammaatan2(-y,-x)# 弧度 - 角度...returnalpha,beta,gamma这里就是把足端坐标(x, y, z)转为alpha髋关节 pitch抬腿/放腿beta膝关节gamma髋关节 yaw腿往外 / 往内摆坐标系和 C 保持一致这里我们先相信原 C 是对的你的后腿问题一般出在下一步的映射。2. polar_to_servo重点defpolar_to_servo(leg,alpha,beta,gamma):ifleg0:alpha90.0-alpha# beta betagammagamma90.0elifleg1:alphaalpha90.0beta180.0-beta gamma90.0-gammaelifleg2:alphaalpha90.0beta180.0-beta gamma90.0-gammaelifleg3:alpha90.0-alpha# beta betagammagamma90.0alphaoffset[leg][0]betaoffset[leg][1]gammaoffset[leg][2]servo_write(leg,0,alpha)servo_write(leg,1,beta)servo_write(leg,2,gamma)这里是“关节角 → 舵机角”的映射核心目的是给每条腿套上不同的“镜像关系”统一让机器人用同一个(alpha, beta, gamma)的逻辑而不管舵机实际安装方式是否镜像你现在遇到的问题是左后腿leg 3的末关节 gamma 方向不对“向里”看一下各腿的 gamma 映射leg 0 (前右)gamma gamma 90leg 1 (后右)gamma 90 - gammaleg 2 (前左)gamma 90 - gammaleg 3 (后左)gamma gamma 90也就是说右后腿 / 左前腿 都用90 - gamma这种形式前右腿 / 后左腿 用的是gamma 90形式如果你实际装机时左后腿的髋关节 yaw 舵机的安装方向和前左腿/后右腿的 yaw 是同一方向那就不应该用gamma 90而应该跟它们一样用90 - gamma。这就解释了为什么只有左后腿末关节怪怪的它现在被当成“和前右腿一样的方向”来控制了。六、修左后腿“末关节向里”的简单改法只要改polar_to_servo()里 leg 3 的 gamma 映射就行。原来的 leg 3 映射elifleg3:alpha90.0-alpha# beta betagammagamma90.0建议改成让它的末关节方向跟 leg 1、leg 2 一致elifleg3:alpha90.0-alpha# 保持不动说明抬腿/放腿方向你觉得是对的# beta beta # 膝关节也先保持不动gamma90.0-gamma# ★ 把原来的 gamma90 换成 90-gamma改完后整体函数就是defpolar_to_servo(leg,alpha,beta,gamma):ifleg0:alpha90.0-alpha gammagamma90.0elifleg1:alphaalpha90.0beta180.0-beta gamma90.0-gammaelifleg2:alphaalpha90.0beta180.0-beta gamma90.0-gammaelifleg3:alpha90.0-alpha gamma90.0-gamma# ← 就改这一行alphaoffset[leg][0]betaoffset[leg][1]gammaoffset[leg][2]servo_write(leg,0,alpha)servo_write(leg,1,beta)servo_write(leg,2,gamma)这样左后腿的 yaw 舵机就跟右后/左前那两个腿的 yaw 方向一致了足端就不会“向里”了而是对称。七、后续微调用 offset 来“磨平”上面改的是“粗结构”方向、零点大偏差搞定之后你可以用offset做细节修比如发现左后腿末关节还是略微朝外 5°就可以offset[[0.0,0.0,0.0],# leg 0[0.0,0.0,0.0],# leg 1[0.0,0.0,0.0],# leg 2[0.0,0.0,-5.0],# leg 3 末关节往里转 5°]然后重启脚本看站立姿态是否更对称。importmath from machineimportPin, PWMimporttime# 舵机驱动层 SERVO_MIN_US500SERVO_MAX_US2500SERVO_FREQ50PWM_MAX_DUTY65535def angle_to_us(angle):ifangle0: angle0elifangle180: angle180usSERVO_MIN_US angle *(SERVO_MAX_US - SERVO_MIN_US)/180.0ifusSERVO_MIN_US: usSERVO_MIN_USelifusSERVO_MAX_US: usSERVO_MAX_USreturnus class ServoDriver: def __init__(self, pin_num,freqSERVO_FREQ): self.pwmPWM(Pin(pin_num),freqfreq)self.pwm.duty_u16(0)def write_angle(self, angle): usangle_to_us(angle)period_us1_000_000 // SERVO_FREQ# 20msdutyint(us / period_us * PWM_MAX_DUTY)ifduty0: duty0elifdutyPWM_MAX_DUTY: dutyPWM_MAX_DUTY self.pwm.duty_u16(duty)# 你的舵机引脚映射保持和 quadruped_walk10.py 完全一致 # leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0[4,5,15],# leg 1[33,25,32],# leg 2[12,14,13],# leg 3]# 名字仅用于打印提示方便你知道自己在调哪个leg_names[FR(前右),RR(后右),FL(前左),RL(后左)]joint_names[关节0(髋外摆),关节1(大腿),关节2(小腿/末端)]# 生成 4x3 ServoDriverservos[[Nonefor_inrange(3)]for_inrange(4)]def init_servos():forleginrange(4):forjinrange(3): servos[leg][j]ServoDriver(servo_pin[leg][j])# 校准逻辑 # 假定理论中位角为 90°BASE_ANGLE90.0# 当前实际舵机角度记录初始化都到 90°current_angle[[BASE_ANGLEfor_inrange(3)]for_inrange(4)]# offset 实际角度 - BASE_ANGLEoffset[[0.0for_inrange(3)]for_inrange(4)]def apply_angle(leg, joint):把 current_angle[leg][joint]写入真实舵机 angcurrent_angle[leg][joint]ifang0: ang0elifang180: ang180servos[leg][joint].write_angle(ang)def update_offset(leg, joint):根据当前角度计算 offset offset[leg][joint]current_angle[leg][joint]- BASE_ANGLE def show_status(current_leg, current_joint): print(\n)print(当前舵机leg {} {} , joint {} {}.format(current_leg, leg_names[current_leg], current_joint, joint_names[current_joint]))print(当前角度: {:.1f}° (理论中位 {}°).format(current_angle[current_leg][current_joint], BASE_ANGLE))print(当前 offset: {:.1f}° 当前角度 - {}.format(offset[current_leg][current_joint], BASE_ANGLE))print(--------------------)print(操作说明)print( n : 下一个舵机)print( p : 上一个舵机)print( : 当前舵机 1°)print( - : 当前舵机 -1°)print( : 当前舵机 5°)print( : 当前舵机 -5°)print( 0 : 当前舵机回到 90°)print( s : 打印所有舵机角度和 offset)print( q : 退出并打印 Python offset 数组)print()def print_all_offsets(): print(\n 当前所有舵机角度 offset )forleginrange(4): print(leg {} {}: .format(leg, leg_names[leg]))forjinrange(3): print( joint {} {}: angle{:.1f}°, offset{:.1f}°.format(j, joint_names[j], current_angle[leg][j], offset[leg][j]))print(\n)def print_offset_array_for_copy(): print(\n 复制到 quadruped_walk10.py 的 offset 数组 )print(offset [)forleginrange(4): row, .join({:.1f}.format(offset[leg][j])forjinrange(3))print( [{row}], # leg {idx} {name}.format(rowrow,idxleg,nameleg_names[leg]))print(]\n)print( 请把上面这段 offset [...] 粘贴到 polar_to_servo 后面的全局变量处 )def main(): print(初始化舵机 ...)init_servos()# 所有舵机先转到理论中位角 90°forleginrange(4):forjinrange(3): apply_angle(leg, j)update_offset(leg, j)time.sleep_ms(50)current_leg0current_joint0show_status(current_leg, current_joint)whileTrue: try: cmdinput(输入命令 (n/p//-///0/s/q): ).strip()except: cmdifnot cmd:continueccmd[0]ifcn:# 下一个舵机idxcurrent_leg *3 current_joint idx(idx 1)%12current_legidx //3current_jointidx %3show_status(current_leg, current_joint)elifcp:# 上一个舵机idxcurrent_leg *3 current_joint idx(idx -1)%12current_legidx //3current_jointidx %3show_status(current_leg, current_joint)elifc:# 微调 1°current_angle[current_leg][current_joint]1.0apply_angle(current_leg, current_joint)update_offset(current_leg, current_joint)show_status(current_leg, current_joint)elifc-:# 微调 -1°current_angle[current_leg][current_joint]-1.0apply_angle(current_leg, current_joint)update_offset(current_leg, current_joint)show_status(current_leg, current_joint)elifc:# 粗调 5°current_angle[current_leg][current_joint]5.0apply_angle(current_leg, current_joint)update_offset(current_leg, current_joint)show_status(current_leg, current_joint)elifc:# 粗调 -5°current_angle[current_leg][current_joint]-5.0apply_angle(current_leg, current_joint)update_offset(current_leg, current_joint)show_status(current_leg, current_joint)elifc0:# 回到理论中位current_angle[current_leg][current_joint]BASE_ANGLE apply_angle(current_leg, current_joint)update_offset(current_leg, current_joint)show_status(current_leg, current_joint)elifcs:print_all_offsets()elifcq:print_all_offsets()print_offset_array_for_copy()print(校准结束。你可以把 offset 数组粘回 gait 程序。)breakelse: print(未知命令, c)show_status(current_leg, current_joint)if__name____main__:main()# quadruped_walk10.py# 基于你提供的 C 代码和几何参数简化为上电 → 站立 → 向前走 N 步# 只保留舵机驱动 逆运动学 轨迹 step_forwardimportmathimporttimefrom machineimportPin, PWM# 舵机驱动层 SERVO_MIN_US500# 与 ESP32Servo attach(500, 2500) 对齐SERVO_MAX_US2500SERVO_FREQ50# 50Hz, 周期 20msPWM_MAX_DUTY65535# duty_u16 的满量程def angle_to_us(angle):角度(0~180)→ 脉宽 us映射0°→500us,180°→2500usifangle0: angle0elifangle180: angle180usSERVO_MIN_US angle *(SERVO_MAX_US - SERVO_MIN_US)/180.0ifusSERVO_MIN_US: usSERVO_MIN_USelifusSERVO_MAX_US: usSERVO_MAX_USreturnus class ServoDriver: def __init__(self, pin_num,freqSERVO_FREQ): self.pwmPWM(Pin(pin_num),freqfreq)self.pwm.duty_u16(0)def write_angle(self, angle): usangle_to_us(angle)period_us1_000_000 // SERVO_FREQ# 20_000 usdutyint(us / period_us * PWM_MAX_DUTY)ifduty0: duty0elifdutyPWM_MAX_DUTY: dutyPWM_MAX_DUTY self.pwm.duty_u16(duty)# 与原 C 一致的引脚映射4 条腿 × 3 舵机# leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0[4,5,15],# leg 1[33,25,32],# leg 2[12,14,13],# leg 3]# 生成 4x3 的 ServoDriver 数组servos[[Nonefor_inrange(3)]for_inrange(4)]def init_servos():forleginrange(4):forjinrange(3): servos[leg][j]ServoDriver(servo_pin[leg][j])def servo_write(leg, joint, angle):封装一层方便以后调整 servos[leg][joint].write_angle(angle)# 机器人几何参数 # 腿几何与你 C 中保持一致length_a55.0# 大腿长度length_b77.5# 小腿长度length_c27.5# 髋关节前向偏移length_side71.0# 身体侧向尺寸这里只是保留不用于直行z_absolute-28.0# 默认站立 / 抬腿高度等z_default-50.0 z_up-30.0 z_bootz_absolute x_default62.0x_offset0.0y_start0.0y_step40.0y_defaultx_default# 运动控制参数KEEP255.0site_now[[0.0,0.0,0.0]for_inrange(4)]# 当前足端 XYZsite_expect[[0.0,0.0,0.0]for_inrange(4)]# 目标足端 XYZtemp_speed[[0.0,0.0,0.0]for_inrange(4)]# 每次迭代 XYZ 增量move_speed1.4speed_multiple1.0spot_turn_speed4.0# 这次不使用leg_move_speed8.0body_move_speed3.0stand_seat_speed1.0# 舵机零位校准偏置原始代码从 EEPROM 中读出来这里先全 0需要你后续自己调offset[[0.0, -5.0,11.0],# leg 0 FR(前右)[0.0,0.0,0.0],# leg 1 RR(后右)[-5.0,5.0, -15.0],# leg 2 FL(前左)[-10.0,10.0,5.0],# leg 3 RL(后左)]pi3.1415926EPS1e-3# 运动学与轨迹函数 def set_site(leg, x, y, z):设定某条腿的目标足端坐标同时根据距离计算每次迭代的速度向量 global temp_speed, site_expect length_x0.0length_y0.0length_z0.0ifx!KEEP: length_xx - site_now[leg][0]ify!KEEP: length_yy - site_now[leg][1]ifz!KEEP: length_zz - site_now[leg][2]lengthmath.sqrt(length_x**2 length_y**2 length_z**2)iflengthEPS:# 目标与当前几乎一样不动temp_speed[leg][0]0.0temp_speed[leg][1]0.0temp_speed[leg][2]0.0else: temp_speed[leg][0]length_x / length * move_speed * speed_multiple temp_speed[leg][1]length_y / length * move_speed * speed_multiple temp_speed[leg][2]length_z / length * move_speed * speed_multipleifx!KEEP: site_expect[leg][0]xify!KEEP: site_expect[leg][1]yifz!KEEP: site_expect[leg][2]z def cartesian_to_polar(x, y, z):足端笛卡尔坐标 → 三关节角度(alpha, beta, gamma)单位度# 对应原 C cartesian_to_polarw(1ifx0else-1)* math.sqrt(x*x y*y)vw - length_c# 避免除零rv2z2v*v z*zifrv2z2EPS: rv2z2EPS alphamath.atan2(z,v) math.acos((length_a*length_a - length_b*length_b v*v z*z)/(2.0* length_a * math.sqrt(rv2z2)))betamath.acos((length_a*length_a length_b*length_b - v*v - z*z)/(2.0* length_a * length_b))ifw0: gammamath.atan2(y, x)else: gammamath.atan2(-y, -x)# 转成角度alphaalpha / pi *180.0betabeta / pi *180.0gammagamma / pi *180.0returnalpha, beta, gamma def polar_to_servo(leg, alpha, beta, gamma): 关节角度 → 舵机角度 使用你代码中“旧版”的简化映射去掉 FRElbow 等动态偏移# 对应你注释掉的那版 polar_to_servoifleg0: alpha90.0- alpha# beta betagammagamma 90.0elifleg1: alphaalpha 90.0beta180.0- beta gamma90.0- gammaelifleg2: alphaalpha 90.0beta180.0- beta gamma90.0- gammaelifleg3: alpha90.0- alpha# beta betagammagamma 90.0# 加上舵机静态校准 offsetalphaoffset[leg][0]betaoffset[leg][1]gammaoffset[leg][2]# 写入舵机servo_write(leg,0, alpha)servo_write(leg,1, beta)servo_write(leg,2, gamma)def servo_service_once():一次“心跳”插值 site_now → 逆解 → 输出舵机角度forleginrange(4):# 插值当前 foot 位置到期望位置foraxisinrange(3):diffsite_expect[leg][axis]- site_now[leg][axis]steptemp_speed[leg][axis]ifabs(diff)abs(step): site_now[leg][axis]step else: site_now[leg][axis]site_expect[leg][axis]# 逆运动学得到三关节角alpha, beta, gammacartesian_to_polar(site_now[leg][0], site_now[leg][1], site_now[leg][2],)# 角度 → 舵机polar_to_servo(leg, alpha, beta, gamma)def leg_reached(leg,epsEPS):foraxisinrange(3):ifabs(site_now[leg][axis]- site_expect[leg][axis])eps:returnFalsereturnTrue def wait_reach(leg):阻塞直到某条腿到达目标位置中间持续调用 servo_service_once()whilenot leg_reached(leg): servo_service_once()time.sleep_ms(10)def wait_all_reach():forleginrange(4): wait_reach(leg)# 姿态与步态函数 def stand():站立所有腿 z → z_default global move_speed move_speedstand_seat_speedforleginrange(4): set_site(leg, KEEP, KEEP, z_default)wait_all_reach()def init_pose():初始化足端坐标类似你 C 里在 setup()里的 set_site 拷贝 site_now# 这里我们直接设定 now 和 expect 一致避免上电大跳init_sites[[x_default - x_offset, y_start y_step, z_boot],# leg 0[x_default - x_offset, y_start y_step, z_boot],# leg 1[x_default x_offset, y_start, z_boot],# leg 2[x_default x_offset, y_start, z_boot],# leg 3]forleginrange(4):foraxisinrange(3): site_now[leg][axis]init_sites[leg][axis]site_expect[leg][axis]init_sites[leg][axis]def step_forward(step_count):向前走 step_count 步对角步态对应你原来的 step_forward() global move_speed move_speedleg_move_speedwhilestep_count0: step_count -1print(step_forward, remain:, step_count)# 注意原代码用 if (site_now[2][1] y_start) 分支# 这里用近似比较避免浮点误差ifabs(site_now[2][1]- y_start)EPS:# 情形 Aleg 2 1 动作# 抬 leg 2set_site(2, x_default x_offset, y_start, z_up)wait_all_reach()# leg 2 前摆set_site(2, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# leg 2 落地set_site(2, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体前移move_speedbody_move_speed set_site(0, x_default x_offset, y_start, z_default)set_site(1, x_default x_offset, y_start 2*y_step, z_default)set_site(2, x_default - x_offset, y_start y_step, z_default)set_site(3, x_default - x_offset, y_start y_step, z_default)wait_all_reach()# 再动 leg 1move_speedleg_move_speed set_site(1, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(1, x_default x_offset, y_start, z_up)wait_all_reach()set_site(1, x_default x_offset, y_start, z_default)wait_all_reach()else:# 情形 Bleg 0 3 动作# 抬 leg 0set_site(0, x_default x_offset, y_start, z_up)wait_all_reach()# leg 0 前摆set_site(0, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# leg 0 落地set_site(0, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体前移move_speedbody_move_speed set_site(0, x_default - x_offset, y_start y_step, z_default)set_site(1, x_default - x_offset, y_start y_step, z_default)set_site(2, x_default x_offset, y_start, z_default)set_site(3, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 再动 leg 3move_speedleg_move_speed set_site(3, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(3, x_default x_offset, y_start, z_up)wait_all_reach()set_site(3, x_default x_offset, y_start, z_default)wait_all_reach()# 主入口 def main(steps): print(Init servos ...)init_servos()print(Init pose ...)init_pose()print(Stand ...)stand()print(Walk forward {} steps ....format(steps))step_forward(steps)print(Done.)# 【这里就是你说的“留一个端口填入 10”的地方】if__name____main__:# 你可以直接在这里改步数比如 10 步steps20# 或者打开下面两行从串口输入一个数字# try:# steps int(input(输入要前进的步数: ))# except:# steps 10main(steps)下面给你一个独立的校准程序专门用来逐个舵机找初始角度“中位”用串口命令微调每个舵机自动计算offset数组并打印出来直接粘进你的quadruped_walk10.py使用说明先看这个把这段代码保存成quadruped_calibrate.py确保和你之前的接线一致servo_pin完全一样烧录到 ESP32运行它打开串口工具115200你会看到操作提示操作键n切到下一个舵机p切到上一个舵机当前舵机1°-当前舵机-1°当前舵机5°当前舵机-5°0当前舵机回到理论中位90°s打印当前所有舵机的角度 offsetq退出循环并打印出Python 格式的offset数组直接复制到你的主程序即可说明程序假设理论中位角 90°你调整到你觉得“正”的姿态时程序自动记为offset 当前角度 - 90你之后在quadruped_walk10.py里把offset [...]换成这里打印的那份即可。校准程序quadruped_calibrate.pyimportmathfrommachineimportPin,PWMimporttime# 舵机驱动层 SERVO_MIN_US500SERVO_MAX_US2500SERVO_FREQ50PWM_MAX_DUTY65535defangle_to_us(angle):ifangle0:angle0elifangle180:angle180usSERVO_MIN_USangle*(SERVO_MAX_US-SERVO_MIN_US)/180.0ifusSERVO_MIN_US:usSERVO_MIN_USelifusSERVO_MAX_US:usSERVO_MAX_USreturnusclassServoDriver:def__init__(self,pin_num,freqSERVO_FREQ):self.pwmPWM(Pin(pin_num),freqfreq)self.pwm.duty_u16(0)defwrite_angle(self,angle):usangle_to_us(angle)period_us1_000_000//SERVO_FREQ# 20msdutyint(us/period_us*PWM_MAX_DUTY)ifduty0:duty0elifdutyPWM_MAX_DUTY:dutyPWM_MAX_DUTY self.pwm.duty_u16(duty)# 你的舵机引脚映射保持和 quadruped_walk10.py 完全一致 # leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0[4,5,15],# leg 1[33,25,32],# leg 2[12,14,13],# leg 3]# 名字仅用于打印提示方便你知道自己在调哪个leg_names[FR(前右),RR(后右),FL(前左),RL(后左)]joint_names[关节0(髋外摆),关节1(大腿),关节2(小腿/末端)]# 生成 4x3 ServoDriverservos[[Nonefor_inrange(3)]for_inrange(4)]definit_servos():forleginrange(4):forjinrange(3):servos[leg][j]ServoDriver(servo_pin[leg][j])# 校准逻辑 # 假定理论中位角为 90°BASE_ANGLE90.0# 当前实际舵机角度记录初始化都到 90°current_angle[[BASE_ANGLEfor_inrange(3)]for_inrange(4)]# offset 实际角度 - BASE_ANGLEoffset[[0.0for_inrange(3)]for_inrange(4)]defapply_angle(leg,joint):把 current_angle[leg][joint] 写入真实舵机angcurrent_angle[leg][joint]ifang0:ang0elifang180:ang180servos[leg][joint].write_angle(ang)defupdate_offset(leg,joint):根据当前角度计算 offsetoffset[leg][joint]current_angle[leg][joint]-BASE_ANGLEdefshow_status(current_leg,current_joint):print(\n)print(当前舵机leg {} {} , joint {} {}.format(current_leg,leg_names[current_leg],current_joint,joint_names[current_joint]))print(当前角度: {:.1f}° (理论中位 {}°).format(current_angle[current_leg][current_joint],BASE_ANGLE))print(当前 offset: {:.1f}° 当前角度 - {}.format(offset[current_leg][current_joint],BASE_ANGLE))print(--------------------)print(操作说明)print( n : 下一个舵机)print( p : 上一个舵机)print( : 当前舵机 1°)print( - : 当前舵机 -1°)print( : 当前舵机 5°)print( : 当前舵机 -5°)print( 0 : 当前舵机回到 90°)print( s : 打印所有舵机角度和 offset)print( q : 退出并打印 Python offset 数组)print()defprint_all_offsets():print(\n 当前所有舵机角度 offset )forleginrange(4):print(leg {} {}: .format(leg,leg_names[leg]))forjinrange(3):print( joint {} {}: angle{:.1f}°, offset{:.1f}°.format(j,joint_names[j],current_angle[leg][j],offset[leg][j]))print(\n)defprint_offset_array_for_copy():print(\n 复制到 quadruped_walk10.py 的 offset 数组 )print(offset [)forleginrange(4):row, .join({:.1f}.format(offset[leg][j])forjinrange(3))print( [{row}], # leg {idx} {name}.format(rowrow,idxleg,nameleg_names[leg]))print(]\n)print( 请把上面这段 offset [...] 粘贴到 polar_to_servo 后面的全局变量处 )defmain():print(初始化舵机 ...)init_servos()# 所有舵机先转到理论中位角 90°forleginrange(4):forjinrange(3):apply_angle(leg,j)update_offset(leg,j)time.sleep_ms(50)current_leg0current_joint0show_status(current_leg,current_joint)whileTrue:try:cmdinput(输入命令 (n/p//-///0/s/q): ).strip()except:cmdifnotcmd:continueccmd[0]ifcn:# 下一个舵机idxcurrent_leg*3current_joint idx(idx1)%12current_legidx//3current_jointidx%3show_status(current_leg,current_joint)elifcp:# 上一个舵机idxcurrent_leg*3current_joint idx(idx-1)%12current_legidx//3current_jointidx%3show_status(current_leg,current_joint)elifc:# 微调 1°current_angle[current_leg][current_joint]1.0apply_angle(current_leg,current_joint)update_offset(current_leg,current_joint)show_status(current_leg,current_joint)elifc-:# 微调 -1°current_angle[current_leg][current_joint]-1.0apply_angle(current_leg,current_joint)update_offset(current_leg,current_joint)show_status(current_leg,current_joint)elifc:# 粗调 5°current_angle[current_leg][current_joint]5.0apply_angle(current_leg,current_joint)update_offset(current_leg,current_joint)show_status(current_leg,current_joint)elifc:# 粗调 -5°current_angle[current_leg][current_joint]-5.0apply_angle(current_leg,current_joint)update_offset(current_leg,current_joint)show_status(current_leg,current_joint)elifc0:# 回到理论中位current_angle[current_leg][current_joint]BASE_ANGLE apply_angle(current_leg,current_joint)update_offset(current_leg,current_joint)show_status(current_leg,current_joint)elifcs:print_all_offsets()elifcq:print_all_offsets()print_offset_array_for_copy()print(校准结束。你可以把 offset 数组粘回 gait 程序。)breakelse:print(未知命令,c)show_status(current_leg,current_joint)if__name____main__:main()接下来怎么用这些 offset运行校准程序调到你满意的姿态。用q退出串口会打印类似offset[[1.0,-3.0,0.0],# leg 0 FR(前右)[2.0,0.0,-1.0],# leg 1 RR(后右)[-1.0,1.0,0.0],# leg 2 FL(前左)[0.0,2.0,1.0],# leg 3 RL(后左)]把这段直接替换你quadruped_walk10.py里面原来的offset[[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0],]之后再跑quadruped_walk10.py姿态和步态就会基于你调好的零位来算整体会规整很多。好我们一步一步来把整个代码和步态关系掰开说一下然后给你一版真正“后腿变前腿”而且不会交叉的step_backward。1. 先把 4 条腿认清楚按你这份代码在这份quadruped_walk10.py里腿编号是你在注释里写的这一套# leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0 前右 FR[4,5,15],# leg 1 后右 RR[33,25,32],# leg 2 前左 FL[12,14,13],# leg 3 后左 RL]再看offset的注释也对应offset[[0.0,-5.0,11.0],# leg 0 FR(前右)[0.0,0.0,0.0],# leg 1 RR(后右)[-5.0,5.0,-15.0],# leg 2 FL(前左)[-10.0,10.0,5.0],# leg 3 RL(后左)]所以物理意义leg0前右FRleg1后右RRleg2前左FLleg3后左RL2. 当前step_forward实际谁先动init_pose()里初始化脚的位置是init_sites[[x_default-x_offset,y_starty_step,z_boot],# leg 0[x_default-x_offset,y_starty_step,z_boot],# leg 1[x_defaultx_offset,y_start,z_boot],# leg 2[x_defaultx_offset,y_start,z_boot],# leg 3]也就是一开始leg0 / leg1 的 y y_start y_stepleg2 / leg3 的 y y_start然后step_forward()一上来判断ifabs(site_now[2][1]-y_start)EPS:刚开始site_now[2][1] y_start于是进情形 A。情形 A 的顺序是抬leg2前左向前摆到y_start 2*y_step身体前移一次性重排 0,1,2,3 到下一个相位再动leg1后右从前方格子摆回中间所以前进的时候的确是「前腿leg2前左所在的对角线前左 后右先动」。3. 你现在这个step_backward的问题你现在贴的step_backward是我上一条给你的版本它有两点跟你想要的逻辑不一致相位判断还在看site_now[2]前左也就是说“谁算是当前相位的基准腿”仍然是前进时的前左腿。在if abs(site_now[2][1] - y_start) EPS的分支里我写的是先动leg0前右也就是还是“物理前腿那一组先动”而你希望的是后退的时候原来的后腿变前腿应该是原来的后腿先动。而且因为我只是简单把 y 方向反过来原来后退方向的“前腿”也就是原来的前腿一开始就向反方向摆这样同侧两条腿在 y 上逼得太近就会出现你说的交叉问题。你现在已经准确抓到了这个 bug 我就不再嘴硬了我们直接改。4. 正确的思路“交换前后腿角色 方向反过来”完全照着step_forward走我们把问题抽象一下在step_forward里虚拟标号是v0FR前右 → 真实 leg0v1RR后右 → 真实 leg1v2FL前左 → 真实 leg2v3RL后左 → 真实 leg3后退时你的要求是逻辑上的“前腿 / 后腿”对调再用和前进一样的步态。也就是后退时的虚拟前后腿应该是“后退方向的前右” 物理的后右 leg1“后退方向的后右” 物理的前右 leg0“后退方向的前左” 物理的后左 leg3“后退方向的后左” 物理的前左 leg2用一个映射写就是虚拟 → 真实v0(FR) - leg1 v1(RR) - leg0 v2(FL) - leg3 v3(RL) - leg2然后我们完全照抄step_forward的逻辑只是把 y 方向的步长改成sy -y_step后退把虚拟腿号 v0,v1,v2,v3 换成上面的真实腿号 1,0,3,2相位判断也要用新的“前左”虚拟 v2对应真实 leg3这样后退的步态一定不会交叉因为就是前进的镜像一定是“后退方向上的前腿先动”也就是原来的后腿5. 一版改好的step_backward完整替换你现在的那个你可以直接把原来的step_backward删掉换成下面这个defstep_backward(step_count):后退步态 - 在逻辑上将前/后腿对调 - y 方向取反sy -y_step - 完全复用 step_forward 的对角步态结构 globalmove_speed move_speedleg_move_speed sy-y_step# 后退方向# 映射说明虚拟 - 真实# v0(FR) - leg1 (原后右)# v1(RR) - leg0 (原前右)# v2(FL) - leg3 (原后左)# v3(RL) - leg2 (原前左)whilestep_count0:step_count-1print(step_backward, remain:,step_count)# 前进时是用 v2前左对应原来的 leg2判断# if abs(site_now[2][1] - y_start) EPS# 后退时新的“前左”是 v2 - 真实 leg3ifabs(site_now[3][1]-y_start)EPS:# 相位 A后退版对应 forward 中的“v2 v1”# 这里是 真实 leg3(v2) 和 leg0(v1) 这一对先动# 即原来的两条后腿在后退方向上变成“前腿”先迈步。# 抬 leg3原后左现在是后退方向的前左set_site(3,x_defaultx_offset,y_start,z_up)wait_all_reach()# 向后退方向摆y_start - y_start 2*sy y_start - 2*y_stepset_site(3,x_defaultx_offset,y_start2*sy,z_up)wait_all_reach()# 落地set_site(3,x_defaultx_offset,y_start2*sy,z_default)wait_all_reach()# 身体后移绝对坐标重排照抄 step_forward 的 v0,v1,v2,v3 布局move_speedbody_move_speed# v0(FR) - leg1set_site(1,x_defaultx_offset,y_start,z_default)# v1(RR) - leg0set_site(0,x_defaultx_offset,y_start2*sy,z_default)# v2(FL) - leg3set_site(3,x_default-x_offset,y_startsy,z_default)# v3(RL) - leg2set_site(2,x_default-x_offset,y_startsy,z_default)wait_all_reach()move_speedleg_move_speed# 再动 v1(RR) - 真实 leg0原前右现在是后退方向的后右set_site(0,x_defaultx_offset,y_start2*sy,z_up)wait_all_reach()set_site(0,x_defaultx_offset,y_start,z_up)wait_all_reach()set_site(0,x_defaultx_offset,y_start,z_default)wait_all_reach()else:# 相位 B后退版对应 forward 中的“v0 v3”# 这里是 真实 leg1(v0) leg2(v3) 动作# 抬 v0(FR) - leg1原后右现在是后退方向的前右set_site(1,x_defaultx_offset,y_start,z_up)wait_all_reach()# 向后退方向摆y_start - y_start 2*sy y_start - 2*y_stepset_site(1,x_defaultx_offset,y_start2*sy,z_up)wait_all_reach()# 落地set_site(1,x_defaultx_offset,y_start2*sy,z_default)wait_all_reach()# 身体后移move_speedbody_move_speed# v0(FR) - leg1set_site(1,x_default-x_offset,y_startsy,z_default)# v1(RR) - leg0set_site(0,x_default-x_offset,y_startsy,z_default)# v2(FL) - leg3set_site(3,x_defaultx_offset,y_start,z_default)# v3(RL) - leg2set_site(2,x_defaultx_offset,y_start2*sy,z_default)wait_all_reach()move_speedleg_move_speed# 再动 v3(RL) - leg2原前左现在是后退方向的后左set_site(2,x_defaultx_offset,y_start2*sy,z_up)wait_all_reach()set_site(2,x_defaultx_offset,y_start,z_up)wait_all_reach()set_site(2,x_defaultx_offset,y_start,z_default)wait_all_reach()6. 预期效果这样改完之后一开始init_posestand后site_now[3][1] y_start所以后退的第一半步会走相位 A也就是先抬原来的两条“后腿中的一条”leg3它在后退方向上是“前腿”先迈步然后再动另一个对角腿leg0整体步态就是前进的镜像。y 方向使用了sy -y_step但所有腿的 y 位置仍然在{y_start, y_start ± y_step, y_start ± 2*y_step}这几个离散层上跳不会出现“某条腿退 3 格另一条退 1 格”那种越走越挤、最后交叉的问题。你可以先这样测试一下流程上电 →init_pose()→stand()step_forward(5)坐下 → 再init_pose()stand()你代码里已经这么写了step_backward(5)# quadruped_walk10.py# 基于你提供的 C 代码和几何参数简化为上电 → 站立 → 向前走 N 步# 只保留舵机驱动 逆运动学 轨迹 step_forwardimportmathimporttimefrom machineimportPin, PWM# 舵机驱动层 SERVO_MIN_US500# 与 ESP32Servo attach(500, 2500) 对齐SERVO_MAX_US2500SERVO_FREQ50# 50Hz, 周期 20msPWM_MAX_DUTY65535# duty_u16 的满量程def angle_to_us(angle):角度(0~180)→ 脉宽 us映射0°→500us,180°→2500usifangle0: angle0elifangle180: angle180usSERVO_MIN_US angle *(SERVO_MAX_US - SERVO_MIN_US)/180.0ifusSERVO_MIN_US: usSERVO_MIN_USelifusSERVO_MAX_US: usSERVO_MAX_USreturnus class ServoDriver: def __init__(self, pin_num,freqSERVO_FREQ): self.pwmPWM(Pin(pin_num),freqfreq)self.pwm.duty_u16(0)def write_angle(self, angle): usangle_to_us(angle)period_us1_000_000 // SERVO_FREQ# 20_000 usdutyint(us / period_us * PWM_MAX_DUTY)ifduty0: duty0elifdutyPWM_MAX_DUTY: dutyPWM_MAX_DUTY self.pwm.duty_u16(duty)# 与原 C 一致的引脚映射4 条腿 × 3 舵机# leg 0: 前右, leg 1: 后右, leg 2: 前左, leg 3: 后左servo_pin[[19,18,23],# leg 0[4,5,15],# leg 1[33,25,32],# leg 2[12,14,13],# leg 3]# 生成 4x3 的 ServoDriver 数组servos[[Nonefor_inrange(3)]for_inrange(4)]def init_servos():forleginrange(4):forjinrange(3): servos[leg][j]ServoDriver(servo_pin[leg][j])def servo_write(leg, joint, angle):封装一层方便以后调整 servos[leg][joint].write_angle(angle)# 机器人几何参数 # 腿几何与你 C 中保持一致length_a55.0# 大腿长度length_b77.5# 小腿长度length_c27.5# 髋关节前向偏移length_side71.0# 身体侧向尺寸这里只是保留不用于直行z_absolute-28.0# 默认站立 / 抬腿高度等z_default-50.0 z_up-30.0 z_bootz_absolute x_default62.0x_offset0.0y_start0.0y_step40.0y_defaultx_default# 运动控制参数KEEP255.0site_now[[0.0,0.0,0.0]for_inrange(4)]# 当前足端 XYZsite_expect[[0.0,0.0,0.0]for_inrange(4)]# 目标足端 XYZtemp_speed[[0.0,0.0,0.0]for_inrange(4)]# 每次迭代 XYZ 增量move_speed1.4speed_multiple1.0spot_turn_speed4.0# 这次不使用leg_move_speed8.0body_move_speed3.0stand_seat_speed1.0# 舵机零位校准偏置原始代码从 EEPROM 中读出来这里先全 0需要你后续自己调offset[[0.0, -5.0,11.0],# leg 0 FR(前右)[0.0,0.0,0.0],# leg 1 RR(后右)[-5.0,5.0, -15.0],# leg 2 FL(前左)[-10.0,10.0,5.0],# leg 3 RL(后左)]pi3.1415926EPS1e-3# 运动学与轨迹函数 def set_site(leg, x, y, z):设定某条腿的目标足端坐标同时根据距离计算每次迭代的速度向量 global temp_speed, site_expect length_x0.0length_y0.0length_z0.0ifx!KEEP: length_xx - site_now[leg][0]ify!KEEP: length_yy - site_now[leg][1]ifz!KEEP: length_zz - site_now[leg][2]lengthmath.sqrt(length_x**2 length_y**2 length_z**2)iflengthEPS:# 目标与当前几乎一样不动temp_speed[leg][0]0.0temp_speed[leg][1]0.0temp_speed[leg][2]0.0else: temp_speed[leg][0]length_x / length * move_speed * speed_multiple temp_speed[leg][1]length_y / length * move_speed * speed_multiple temp_speed[leg][2]length_z / length * move_speed * speed_multipleifx!KEEP: site_expect[leg][0]xify!KEEP: site_expect[leg][1]yifz!KEEP: site_expect[leg][2]z def cartesian_to_polar(x, y, z):足端笛卡尔坐标 → 三关节角度(alpha, beta, gamma)单位度# 对应原 C cartesian_to_polarw(1ifx0else-1)* math.sqrt(x*x y*y)vw - length_c# 避免除零rv2z2v*v z*zifrv2z2EPS: rv2z2EPS alphamath.atan2(z,v) math.acos((length_a*length_a - length_b*length_b v*v z*z)/(2.0* length_a * math.sqrt(rv2z2)))betamath.acos((length_a*length_a length_b*length_b - v*v - z*z)/(2.0* length_a * length_b))ifw0: gammamath.atan2(y, x)else: gammamath.atan2(-y, -x)# 转成角度alphaalpha / pi *180.0betabeta / pi *180.0gammagamma / pi *180.0returnalpha, beta, gamma def polar_to_servo(leg, alpha, beta, gamma): 关节角度 → 舵机角度 使用你代码中“旧版”的简化映射去掉 FRElbow 等动态偏移# 对应你注释掉的那版 polar_to_servoifleg0: alpha90.0- alpha# beta betagammagamma 90.0elifleg1: alphaalpha 90.0beta180.0- beta gamma90.0- gammaelifleg2: alphaalpha 90.0beta180.0- beta gamma90.0- gammaelifleg3: alpha90.0- alpha# beta betagammagamma 90.0# 加上舵机静态校准 offsetalphaoffset[leg][0]betaoffset[leg][1]gammaoffset[leg][2]# 写入舵机servo_write(leg,0, alpha)servo_write(leg,1, beta)servo_write(leg,2, gamma)def servo_service_once():一次“心跳”插值 site_now → 逆解 → 输出舵机角度forleginrange(4):# 插值当前 foot 位置到期望位置foraxisinrange(3):diffsite_expect[leg][axis]- site_now[leg][axis]steptemp_speed[leg][axis]ifabs(diff)abs(step): site_now[leg][axis]step else: site_now[leg][axis]site_expect[leg][axis]# 逆运动学得到三关节角alpha, beta, gammacartesian_to_polar(site_now[leg][0], site_now[leg][1], site_now[leg][2],)# 角度 → 舵机polar_to_servo(leg, alpha, beta, gamma)def leg_reached(leg,epsEPS):foraxisinrange(3):ifabs(site_now[leg][axis]- site_expect[leg][axis])eps:returnFalsereturnTrue def wait_reach(leg):阻塞直到某条腿到达目标位置中间持续调用 servo_service_once()whilenot leg_reached(leg): servo_service_once()time.sleep_ms(10)def wait_all_reach():forleginrange(4): wait_reach(leg)# 姿态与步态函数 def stand():站立所有腿 z → z_default global move_speed move_speedstand_seat_speedforleginrange(4): set_site(leg, KEEP, KEEP, z_default)wait_all_reach()def seat():坐下 / 收腿回到类似上电姿态脚抬高一些 global move_speed move_speedstand_seat_speed# 这里参考 init_pose 的 x、y只是把 z 调到 z_boot比较“蹲”target_sites[[x_default - x_offset, y_start y_step, z_boot],# leg 0[x_default - x_offset, y_start y_step, z_boot],# leg 1[x_default x_offset, y_start, z_boot],# leg 2[x_default x_offset, y_start, z_boot],# leg 3]forleginrange(4): set_site(leg, target_sites[leg][0], target_sites[leg][1], target_sites[leg][2])wait_all_reach()def init_pose():初始化足端坐标类似你 C 里在 setup()里的 set_site 拷贝 site_now# 这里我们直接设定 now 和 expect 一致避免上电大跳init_sites[[x_default - x_offset, y_start y_step, z_boot],# leg 0[x_default - x_offset, y_start y_step, z_boot],# leg 1[x_default x_offset, y_start, z_boot],# leg 2[x_default x_offset, y_start, z_boot],# leg 3]forleginrange(4):foraxisinrange(3): site_now[leg][axis]init_sites[leg][axis]site_expect[leg][axis]init_sites[leg][axis]def step_forward(step_count):向前走 step_count 步对角步态对应你原来的 step_forward() global move_speed move_speedleg_move_speedwhilestep_count0: step_count -1print(step_forward, remain:, step_count)# 注意原代码用 if (site_now[2][1] y_start) 分支# 这里用近似比较避免浮点误差ifabs(site_now[2][1]- y_start)EPS:# 情形 Aleg 2 1 动作# 抬 leg 2set_site(2, x_default x_offset, y_start, z_up)wait_all_reach()# leg 2 前摆set_site(2, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# leg 2 落地set_site(2, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体前移move_speedbody_move_speed set_site(0, x_default x_offset, y_start, z_default)set_site(1, x_default x_offset, y_start 2*y_step, z_default)set_site(2, x_default - x_offset, y_start y_step, z_default)set_site(3, x_default - x_offset, y_start y_step, z_default)wait_all_reach()# 再动 leg 1move_speedleg_move_speed set_site(1, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(1, x_default x_offset, y_start, z_up)wait_all_reach()set_site(1, x_default x_offset, y_start, z_default)wait_all_reach()else:# 情形 Bleg 0 3 动作# 抬 leg 0set_site(0, x_default x_offset, y_start, z_up)wait_all_reach()# leg 0 前摆set_site(0, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# leg 0 落地set_site(0, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体前移move_speedbody_move_speed set_site(0, x_default - x_offset, y_start y_step, z_default)set_site(1, x_default - x_offset, y_start y_step, z_default)set_site(2, x_default x_offset, y_start, z_default)set_site(3, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 再动 leg 3move_speedleg_move_speed set_site(3, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(3, x_default x_offset, y_start, z_up)wait_all_reach()set_site(3, x_default x_offset, y_start, z_default)wait_all_reach()def step_backward(step_count):正确的后退交换前后腿角色 完全复用 step_forward 逻辑方向不用反 global move_speed move_speedleg_move_speed# 映射虚拟→真实# v0 原 leg1后右# v1 原 leg0前右# v2 原 leg3后左# v3 原 leg2前左v[1,0,3,2]# 虚拟腿号对应的真实腿号whilestep_count0: step_count -1print(step_backward, remain:, step_count)# 和 forward 一样但基准虚拟腿 v2 真实 leg3ifabs(site_now[v[2]][1]- y_start)EPS:## 相位 A虚拟 v2 v1对应真实 leg3 和 leg0#L2v[2]# leg3L1v[1]# leg0# 抬 L2set_site(L2, x_default x_offset, y_start, z_up)wait_all_reach()# 前摆方向不变set_site(L2, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# 落地set_site(L2, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体移动完全照抄 forwardmove_speedbody_move_speed set_site(v[0], x_default x_offset, y_start, z_default)set_site(v[1], x_default x_offset, y_start 2*y_step, z_default)set_site(v[2], x_default - x_offset, y_start y_step, z_default)set_site(v[3], x_default - x_offset, y_start y_step, z_default)wait_all_reach()# 再动 L1move_speedleg_move_speed set_site(L1, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(L1, x_default x_offset, y_start, z_up)wait_all_reach()set_site(L1, x_default x_offset, y_start, z_default)wait_all_reach()else:## 相位 B虚拟 v0 v3对应真实 leg1 和 leg2#L0v[0]# leg1L3v[3]# leg2# 抬 L0set_site(L0, x_default x_offset, y_start, z_up)wait_all_reach()# 前摆方向不变set_site(L0, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()# 落地set_site(L0, x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 身体移动move_speedbody_move_speed set_site(v[0], x_default - x_offset, y_start y_step, z_default)set_site(v[1], x_default - x_offset, y_start y_step, z_default)set_site(v[2], x_default x_offset, y_start, z_default)set_site(v[3], x_default x_offset, y_start 2*y_step, z_default)wait_all_reach()# 再动 L3move_speedleg_move_speed set_site(L3, x_default x_offset, y_start 2*y_step, z_up)wait_all_reach()set_site(L3, x_default x_offset, y_start, z_up)wait_all_reach()set_site(L3, x_default x_offset, y_start, z_default)wait_all_reach()# 主入口 def main(steps): print(Init servos ...)init_servos()print(Init pose ...)init_pose()print(Stand ...)stand()print(Walk forward {} steps ....format(steps))step_forward(steps)# 稍微停一下防止来回太急time.sleep(1)print(Seat ...)seat()time.sleep(1)print(Init servos ...)init_servos()print(Init pose ...)init_pose()print(Stand ...)stand()time.sleep(1)print(Walk backward {} steps ....format(steps))step_backward(steps)# 再停一下time.sleep(1)print(Seat ...)seat()print(Done.)# 【这里就是你说的“留一个端口填入 10”的地方】if__name____main__:# 你可以直接在这里改步数比如 10 步steps5# 或者打开下面两行从串口输入一个数字# try:# steps int(input(输入要前进的步数: ))# except:# steps 10main(steps)