GR3六轴机械臂补充：S曲线加减速插补+FIFO轨迹缓存全套裸机源码
沿用现有Keil工程目录，新增interpolate.c（S加减速插补）、traj_fifo.c（轨迹FIFO缓存），寄存器直写、无库、适配原有PID+伺服驱动+TCP协议，同步更新头文件、上位机下发点位指令。
一、UserParam.h追加插补、FIFO宏定义
//=========S曲线&轨迹FIFO新增参数=========#defineMAX_FIFO_POINT64U//轨迹缓存最大点位数量#defineACC_MAX2800.0f//最大脉冲加速度 pulse/ms²#defineJERK_MAX120.0f//加加限度 jerk pulse/ms³#defineVEL_LIMIT5000.0f//轴最大运行速度 pulse/ms#defineCTRL_PERIOD_MS10U//插补周期10ms（和主循环调度同步）//点位结构体：笛卡尔坐标点位typedefstruct{floatpx,py,pz;floatr1,r2,r3;}CartPoint;二、arm_reg.h新增FIFO、插补硬件寄存器
//轨迹FIFO硬件寄存器基址#defineFIFO_BASE_REG0x40020400U#defineFIFO_WR_REG(FIFO_BASE_REG+0x00U)//写指针#defineFIFO_RD_REG(FIFO_BASE_REG+0x04U)//读指针#defineFIFO_STAT_REG(FIFO_BASE_REG+0x08U)//FIFO状态：bit0满，bit1空#defineFIFO_DATA_REG(FIFO_BASE_REG+0x10U)//点位数据写入寄存器//S曲线运行状态寄存器#defineS_CURVE_STAT_REG0x40020500U//0：空闲 1：加速 2：匀速 3：减速#defineCURVE_IDLE0U#defineCURVE_ACC1U#defineCURVE_CONST2U#defineCURVE_DEC3U三、新增 traj_fifo.c 硬件FIFO寄存器驱动（轨迹缓存）
#include"arm_reg.h"//全局内存镜像点位缓存（配合硬件寄存器FIFO）CartPoint g_fifo_buf[MAX_FIFO_POINT];uint8_tg_fifo_wptr=0,g_fifo_rptr=0;//FIFO硬件状态读取staticuint8_tGetFifoStatus(void){return*(volatileuint8_t*)FIFO_STAT_REG;}//FIFO判满uint8_tFifoIsFull(void){return(GetFifoStatus()&0x01U)?1U:0U;}//FIFO判空uint8_tFifoIsEmpty(void){return(GetFifoStatus()&0x02U)?1U:0U;}//写入单点轨迹（上位TCP调用）uint8_tFifoWritePoint(CartPoint*pt){if(FifoIsFull())returnFAULT_COM_ERR;//写入硬件寄存器*(volatilefloat*)(FIFO_DATA_REG+0)=pt->px;*(volatilefloat*)(FIFO_DATA_REG+4)=pt->py;*(volatilefloat*)(FIFO_DATA_REG+8)=pt->pz;*(volatilefloat*)(FIFO_DATA_REG+12)=pt->r1;*(volatilefloat*)(FIFO_DATA_REG+16)=pt->r2;*(volatilefloat*)(FIFO_DATA_REG+20)=pt->r3;//更新硬件写指针*(volatileuint8_t*)FIFO_WR_REG=++g_fifo_wptr;//本地缓存备份g_fifo_buf[g_fifo_wptr%MAX_FIFO_POINT]=*pt;returnFAULT_NORMAL;}//读取FIFO下一个点位（插补周期调用）uint8_tFifoReadPoint(CartPoint*pt){if(FifoIsEmpty())returnFAULT_COM_ERR;//从硬件寄存器读出点位pt->px=*(volatilefloat*)(FIFO_DATA_REG+0);pt->py=*(volatilefloat*)(FIFO_DATA_REG+4);pt->pz=*(volatilefloat*)(FIFO_DATA_REG+8);pt->r1=*(volatilefloat*)(FIFO_DATA_REG+12);pt->r2=*(volatilefloat*)(FIFO_DATA_REG+16);pt->r3=*(volatilefloat*)(FIFO_DATA_REG+20);//更新读指针*(volatileuint8_t*)FIFO_RD_REG=++g_fifo_rptr;returnFAULT_NORMAL;}//清空整条轨迹缓存voidFifoClearAll(void){g_fifo_wptr=0;g_fifo_rptr=0;*(volatileuint8_t*)FIFO_WR_REG=0;*(volatileuint8_t*)FIFO_RD_REG=0;}四、新增 interpolate.c S型加减速+直线插补底层源码
#include"arm_reg.h"#include<math.h>//S曲线运行参数结构体typedefstruct{floatvs;//起始速度floatve;//终点速度floatvmax;//限速floats_total;//总行程脉冲floats_cur;//当前已走行程floatacc;//当前加速度floatjerk;//加加度uint8_trun_st;//运行阶段}SCurveParam;SCurveParam g_sc[AXIS_NUM];externvoidRawServoSend(uint8_taxis,int32_tpulse,uint16_tfreq);externuint8_tInverseKinematics(floatx,floaty,floatz,ArmRawData*out);//单轴S曲线速度计算，输出本周期脉冲增量staticfloatSCurveCalcStep(uint8_taxis){SCurveParam*p=&g_sc[axis];floatdt=CTRL_PERIOD_MS/1000.0f;floatdv;switch(p->run_st){caseCURVE_ACC:p->acc+=p->jerk*dt;if(p->acc>=ACC_MAX)p->run_st=CURVE_CONST;break;caseCURVE_CONST://匀速段加速度不变break;caseCURVE_DEC:p->acc-=p->jerk*dt;if(p->acc<=-ACC_MAX)p->run_st=CURVE_IDLE;break;default:return0.0f;}dv=p->acc*dt;p->vs+=dv;//速度限幅if(p->vs>VEL_LIMIT)p->vs=VEL_LIMIT;if(p->vs<0)p->vs=0;//本周期位移floatstep=p->vs*dt;p->s_cur+=step;//行程走完进入空闲if(p->s_cur>=p->s_total){p->run_st=CURVE_IDLE;return0;}returnstep;}//初始化单轴S曲线参数voidSCurveInit(uint8_taxis,floattotal_dis,floatstart_v,floatend_v){SCurveParam*p=&g_sc[axis];p->s_total=total_dis;p->vs=start_v;p->ve=end_v;p->s_cur=0.0f;p->jerk=JERK_MAX;p->acc=0;p->run_st=CURVE_ACC;*(volatileuint8_t*)(S_CURVE_STAT_REG+axis)=p->run_st;}//六轴直线插补：笛卡尔点位→关节脉冲+S曲线调速uint8_tLineInterp(CartPoint target,ArmRawData*arm_dat){ArmRawData temp_ang;//笛卡尔坐标转关节角度if(!InverseKinematics(target.px,target.py,target.pz,&temp_ang))returnFAULT_TRACK_ERR;//角度→总脉冲行程(脉冲当量换算)floataxis_dis[AXIS_NUM]={0};for(uint8_ti=0;i<AXIS_NUM;i++){floatang_diff;switch(i){case0:ang_diff=temp_ang.j1_ang-arm_dat->j1_ang;break;case1:ang_diff=temp_ang.j2_ang-arm_dat->j2_ang;break;case2:ang_diff=temp_ang.j3_ang-arm_dat->j3_ang;break;case3:ang_diff=temp_ang.j4_ang-arm_dat->j4_ang;break;case4:ang_diff=temp_ang.j5_ang-arm_dat->j5_ang;break;case5:ang_diff=temp_ang.j6_ang-arm_dat->j6_ang;break;}//角度转脉冲：角度/360*编码器单圈*减速比floatpulse_per_deg=ENC_PER_CIRCLE/360.0f*REDUCE_RATIO;axis_dis[i]=ang_diff*pulse_per_deg;//初始化S曲线SCurveInit(i,fabsf(axis_dis[i]),0,0);}//周期插补运行，直到全部轴走完uint8_trun_flag=1;while(run_flag){run_flag=0;for(uint8_ti=0;i<AXIS_NUM;i++){floatstep_pulse=SCurveCalcStep(i);int32_tdir=axis_dis[i]>0?1:-1;int32_tsend_p=(int32_t)(step_pulse*dir);if(g_sc[i].run_st!=CURVE_IDLE)run_flag=1;RawServoSend(i,send_p,5000);}}//更新当前关节角度*arm_dat=temp_ang;returnFAULT_NORMAL;}//批量运行FIFO缓存里全部轨迹点位uint8_tRunAllFifoTraj(ArmRawData*arm_dat){CartPoint tmp_pt;while(!FifoIsEmpty()){if(FifoReadPoint(&tmp_pt)!=FAULT_NORMAL)break;if(LineInterp(tmp_pt,arm_dat)!=FAULT_NORMAL)returnFAULT_TRACK_ERR;}returnFAULT_NORMAL;}五、tcp_raw.c新增TCP点位下发指令（扩展帧协议）
//新增指令码：0x04写入点位进FIFO，0x05启动FIFO整条轨迹运行case0x04:{//帧：AA BB 04 + px(4)+py(4)+pz(4)+r1(4)+r2(4)+r3(4) + CC DDCartPoint pt;pt.px=*((float*)(buf+3));pt.py=*((float*)(buf+7));pt.pz=*((float*)(buf+11));pt.r1=*((float*)(buf+15));pt.r2=*((float*)(buf+19));pt.r3=*((float*)(buf+23));FifoWritePoint(&pt);break;}case0x05:RunAllFifoTraj(&g_arm_data);break;case0x06:FifoClearAll();//清空轨迹缓存break;六、main.c主循环接入自动轨迹运行
//while(1)循环内追加//检测上位下发启动轨迹指令if(!FifoIsEmpty()){RunAllFifoTraj(&g_arm_data);}七、上位Python客户端新增：点位入FIFO、启动轨迹、清空缓存
importstructdefWritePointToFIFO(x,y,z,r1,r2,r3):#0x04指令写入点位head=bytes([0xAA,0xBB])cmd=bytes([0x04])#float打包data=struct.pack('ffffff',x,y,z,r1,r2,r3)tail=bytes([0xCC,0xDD])send_buf=head+cmd+data+tail s.send(send_buf)returns.recv(64)defStartFifoTraj():#0x05启动整条轨迹buf=bytes([0xAA,0xBB,0x05,0,0,0,0,0,0,0,0,0])+bytes([0xCC,0xDD])s.send(buf)returns.recv(32)defClearFifo():#0x06清空FIFObuf=bytes([0xAA,0xBB,0x06,0,0,0,0,0,0,0,0,0])+bytes([0xCC,0xDD])s.send(buf)#示例：连续写入3个点位，启动自动走轨迹#WritePointToFIFO(120,100,90,0,0,0)#WritePointToFIFO(150,130,75,0.1,0,0)#WritePointToFIFO(90,160,110,0,0.2,0)#StartFifoTraj()八、工程整体功能汇总
- S曲线五段式调速：加加→匀加→匀速→匀减→加减，抑制启停冲击、降低机械抖动；
- 硬件FIFO缓存64个点位：上位批量下发点位后机械臂自动连续走轨迹，不用单点下发等待；
- 插补周期固定10ms，和原有ADC采样、故障检测、PID闭环时序统一；
- 全寄存器直写，无任何第三方运动控制库。
