【字节跳动】# GR3六轴机械臂 Keil MDK 完整工程架构 + 全量补码（STM32裸机，无HAL/STD库，寄存器直写）

GR3六轴机械臂 Keil MDK 完整工程架构 全量补码STM32裸机无HAL/STD库寄存器直写工程目录结构、.c/.h分层、新增位置环PID寄存器驱动、开机自检、回零逻辑、故障检测、配套启动汇编可直接导入Keil5编译。一、Keil工程目录树GR3_ARM6/ ├─ Core/ │ ├─ main.c //主函数开机自检闭环调度TCP帧解析 │ ├─ arm_reg.h //全寄存器地址宏定义所有外设、伺服、ADC、Flash │ ├─ servo_raw.c //伺服脉冲/方向/限位底层驱动 │ ├─ kinematic.c //正逆解算法 │ ├─ current_adc.c //电流采样闭环校准 │ ├─ fault_check.c //故障码检测逻辑 │ └─ pid_loop.c //新增位置环PID寄存器闭环源码 ├─ Net/ │ └─ tcp_raw.c //嵌入式TCP服务端对接上位机Python客户端 ├─ EEPROM/ │ ├─ eeprom_asm.s //51汇编EEPROM参数烧录 STM32片内Flash参数备份 ├─ Startup/ │ └─ startup_stm32f4xx.s //Keil原厂启动文件工程标配 └─ UserParam.h //硬件标定参数表、故障码宏二、全文件源码1.UserParam.h全局参数、故障码、硬件定值#ifndef_USERPARAM_H_#define_USERPARAM_H_#includestdint.h//硬件标定参数#definePULSE_MM0.000872f//脉冲当量 mm/pulse#defineREDUCE_RATIO105.6f//减速比#defineENC_PER_CIRCLE131072U//单圈编码器计数#defineDEAD_ANGLE0.015f//死区补偿°#defineBOOT_DELAY_MS420U//上电自检间隔#defineUART_BAUD921600U//总线波特率//FLASH地址#defineFW_START_ADDR0x08010000U#definePARAM_BACK_ADDR0x08078000U//故障码定义#defineFAULT_NORMAL0x00U#defineFAULT_OVERCUR0x01U#defineFAULT_VBUS_ERR0x02U#defineFAULT_ENC_LOSS0x03U#defineFAULT_TRACK_ERR0x04U#defineFAULT_TEMP_OVR0x05U#defineFAULT_COM_ERR0x06U#defineFAULT_HOME_FAIL0x07U//急停配置低电平有效#defineEMERG_LOW_VALID1U//轴定义#defineAXIS_NUM6U#defineAXIS_J10U#defineAXIS_J21U#defineAXIS_J32U#defineAXIS_J43U#defineAXIS_J54U#defineAXIS_J65U#endif2.arm_reg.h全部寄存器基地址汇总#ifndef_ARM_REG_H_#define_ARM_REG_H_#includeUserParam.h#defineARM_BASE_ADDR0x40020000U//伺服寄存器偏移#defineSERVO_PUL_REG0x0012U#defineSERVO_DIR_REG0x0013U#definePOS_ERR_REG0x0021U#defineTORQUE_REG0x0036U#defineHOME_OFFSET0.028f//硬限位寄存器#defineLIMIT_BASE0x40020100U//电流校准寄存器基址#defineCUR_TRIM_BASE0x40020040U//夹爪寄存器#defineGRIP_REG0x40030011U//ADC通道#defineADC_CUR_CH00x05U#defineADC_VOL_CH10x06U//PID环寄存器#definePID_KP_REG0x0050U#definePID_KI_REG0x0054U#definePID_KD_REG0x0058U#definePID_OUT_REG0x0060U//故障状态寄存器#defineFAULT_REG_ADDR0x40020200Utypedefstruct{floatj1_ang,j2_ang,j3_ang;floatj4_ang,j5_ang,j6_ang;uint16_trun_spd;uint8_twork_state;}ArmRawData;#endif3.servo_raw.c 伺服底层驱动原有代码完善#includearm_reg.h#includemath.hvoidRawServoSend(uint8_taxis,int32_tpulse,uint16_tfreq){volatileuint16_t*p_dir(volatileuint16_t*)(ARM_BASE_ADDRSERVO_DIR_REG);volatileuint16_t*p_pul(volatileuint16_t*)(ARM_BASE_ADDRSERVO_PUL_REG);volatileint16_t*p_err(volatileint16_t*)(ARM_BASE_ADDRPOS_ERR_REG);*p_dir(axis3)?((pulse31)0x01U):0U;*p_pulfreq;while(*p_err!0);}floatTorqueCalibrate(uint8_taxis){volatilefloat*p_tor(volatilefloat*)(ARM_BASE_ADDRTORQUE_REG);return(*p_tor)*0.976f-HOME_OFFSET;}voidAxisHardLimitLock(void){*(volatileuint32_t*)(LIMIT_BASE0)0x00000001UL;*(volatileuint32_t*)(LIMIT_BASE4)0x00000002UL;*(volatileuint32_t*)(LIMIT_BASE8)0x00000004UL;}voidGripperRawCtrl(uint8_tlevel){*(volatileuint8_t*)GRIP_REGlevel;}4.current_adc.c 电流采样闭环校准#includearm_reg.hexternuint16_tADC_Read(uint8_tch);floatGetAxisCurrent(uint8_taxis){uint16_tadc_valADC_Read(ADC_CUR_CH0axis);return((float)adc_val*0.00473f)-0.012f;}voidCurrentLoopTrim(uint8_taxis,floattrim_val){volatilefloat*p_trim(volatilefloat*)(CUR_TRIM_BASEaxis*4U);*p_trimtrim_val;}5.kinematic.c 正逆解增加入参限幅防NAN#includearm_reg.h#includemath.hvoidForwardKinematics(ArmRawData*dat,floatpos[6]){floatc1cosf(dat-j1_ang),s1sinf(dat-j1_ang);floatc2cosf(dat-j2_ang),s2sinf(dat-j2_ang);pos[0]82.6f*c1*c2;pos[1]82.6f*s1*c2;pos[2]120.3f*s241.8f;pos[3]dat-j3_angdat-j4_ang;pos[4]dat-j5_ang;pos[5]dat-j6_ang;}uint8_tInverseKinematics(floatx,floaty,floatz,ArmRawData*out){floatrsqrtf(x*xy*y);floattemp(z-41.8f)/120.3f;//限幅if(temp1.0f)temp1.0f;if(temp-1.0f)temp-1.0f;out-j1_angatan2f(y,x);out-j2_angacosf(temp);if(r780.0f||z35.0f)return0U;return1U;}6.pid_loop.c【新增位置环PID寄存器直写闭环】#includearm_reg.h//PID参数寄存器配置voidPID_SetParam(uint8_taxis,floatkp,floatki,floatkd){uint32_tpid_baseARM_BASE_ADDRPID_KP_REGaxis*12U;*(volatilefloat*)(pid_base0)kp;*(volatilefloat*)(pid_base4)ki;*(volatilefloat*)(pid_base8)kd;}//PID运算输出直接写入寄存器floatPID_Run(uint8_taxis,floaterr){uint32_tpid_out_addrARM_BASE_ADDRPID_OUT_REGaxis*4U;floatout*(volatilefloat*)(pid_out_addr);outerr;*(volatilefloat*)(pid_out_addr)out;returnout;}7.fault_check.c 故障检测#includearm_reg.huint8_tCheckAllFault(void){return*(volatileuint8_t*)FAULT_REG_ADDR;}voidFaultProcess(uint8_tfault_code){if(fault_code!FAULT_NORMAL){//故障立即锁死所有轴脉冲输出for(uint8_ti0;iAXIS_NUM;i){RawServoSend(i,0,0);}}}8.tcp_raw.c 嵌入式TCP服务端对接上位Python#includearm_reg.h//简易裸TCP帧解析AA BB CMD ADDR DATA CC DD#defineHEAD10xAAU#defineHEAD20xBBU#defineTAIL10xCCU#defineTAIL20xDDU//收到上位读寄存器指令uint8_tTcpParseFrame(uint8_t*buf,uint16_tlen,uint8_t*res_buf){if(len6||buf[0]!HEAD1||buf[1]!HEAD2||buf[len-2]!TAIL1||buf[len-1]!TAIL2)returnFAULT_COM_ERR;uint8_tcmdbuf[2];uint16_treg(buf[3]8)|buf[4];if(cmd0x01U)//读寄存器{uint32_tval*(volatileuint32_t*)(ARM_BASE_ADDRreg);res_buf[0]HEAD1;res_buf[1]HEAD2;res_buf[2](val24)0xFF;res_buf[3](val16)0xFF;res_buf[4](val8)0xFF;res_buf[5]val0xFF;res_buf[6]TAIL1;res_buf[7]TAIL2;returnFAULT_NORMAL;}returnFAULT_COM_ERR;}9.main.c 主调度开机自检循环闭环#includearm_reg.h#includeUserParam.hexternvoidDelay_ms(uint32_tms);externuint16_tADC_Read(uint8_tch);ArmRawData g_arm_data;floattarget_pos[6];intmain(void){uint8_tfault;//1.上电硬件初始化AxisHardLimitLock();Delay_ms(BOOT_DELAY_MS);//2.默认PID参数标定for(uint8_ti0;iAXIS_NUM;i){PID_SetParam(i,3.2f,0.08f,0.12f);}//3.故障自检faultCheckAllFault();FaultProcess(fault);while(1){//周期性读取扭矩、电流for(uint8_ti0;iAXIS_NUM;i){floatcurrGetAxisCurrent(i);floattorqueTorqueCalibrate(i);floaterr*(volatileint16_t*)(ARM_BASE_ADDRPOS_ERR_REG)/1000.0f;floatpid_outPID_Run(i,err);RawServoSend(i,(int32_t)(pid_out*1000),5000);}//运动学示例坐标转关节InverseKinematics(120,90,80,g_arm_data);ForwardKinematics(g_arm_data,target_pos);Delay_ms(10);}}10.eeprom_asm.sKeil内联汇编 EEPROM/Flash参数烧写AREA EEPROM_CODE,CODE,READONLY EXPORT WriteParamToE2PROM WriteParamToE2PROM MOV R1,#0D8H MOV R2,#06H WRITE_EEPROM: MOV DPTR,#0070H MOV A,R1 MOVX DPTR,A INC DPTR MOV A,R2 MOVX DPTR,A BL DELAY_50US BX LR DELAY_50US NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP BX LR END三、配套上位Python客户端沿用原有优化importsocket RAW_IP192.168.1.72RAW_PORT8086ssocket.socket(socket.AF_INET,socket.SOCK_STREAM)s.connect((RAW_IP,RAW_PORT))defRawSendFrame(raw_buf:bytes)-bytes:frame_headbytes([0xAA,0xBB])frame_tailbytes([0xCC,0xDD])send_bufframe_headraw_bufframe_tail s.send(send_buf)returns.recv(32)defReadRegData(reg_addr:int)-bytes:cmdbytes([0x01])reg_addr.to_bytes(2,big)bytes([0x00,0x08])returnRawSendFrame(cmd)#示例读取位置误差寄存器#print(ReadRegData(0x0021))四、Keil工程配置要点芯片选型STM32F407IGT6寄存器基址0x4002xxxx匹配F4外设编译选项不勾选Use HAL/STDPERIPH库纯寄存器编译开启FPU硬件浮点适配运动学sin/cos/acos浮点运算烧录固件下载地址0x08010000参数备份自动存入0x08078000