【字节跳动】# GR3六轴机械臂补充：S曲线加减速插补+FIFO轨迹缓存全套裸机源码

GR3六轴机械臂补充S曲线加减速插补FIFO轨迹缓存全套裸机源码沿用现有Keil工程目录新增interpolate.cS加减速插补、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_REG0x00U)//写指针#defineFIFO_RD_REG(FIFO_BASE_REG0x04U)//读指针#defineFIFO_STAT_REG(FIFO_BASE_REG0x08U)//FIFO状态bit0满bit1空#defineFIFO_DATA_REG(FIFO_BASE_REG0x10U)//点位数据写入寄存器//S曲线运行状态寄存器#defineS_CURVE_STAT_REG0x40020500U//0空闲 1加速 2匀速 3减速#defineCURVE_IDLE0U#defineCURVE_ACC1U#defineCURVE_CONST2U#defineCURVE_DEC3U三、新增 traj_fifo.c 硬件FIFO寄存器驱动轨迹缓存#includearm_reg.h//全局内存镜像点位缓存配合硬件寄存器FIFOCartPoint g_fifo_buf[MAX_FIFO_POINT];uint8_tg_fifo_wptr0,g_fifo_rptr0;//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_REG0)pt-px;*(volatilefloat*)(FIFO_DATA_REG4)pt-py;*(volatilefloat*)(FIFO_DATA_REG8)pt-pz;*(volatilefloat*)(FIFO_DATA_REG12)pt-r1;*(volatilefloat*)(FIFO_DATA_REG16)pt-r2;*(volatilefloat*)(FIFO_DATA_REG20)pt-r3;//更新硬件写指针*(volatileuint8_t*)FIFO_WR_REGg_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_REG0);pt-py*(volatilefloat*)(FIFO_DATA_REG4);pt-pz*(volatilefloat*)(FIFO_DATA_REG8);pt-r1*(volatilefloat*)(FIFO_DATA_REG12);pt-r2*(volatilefloat*)(FIFO_DATA_REG16);pt-r3*(volatilefloat*)(FIFO_DATA_REG20);//更新读指针*(volatileuint8_t*)FIFO_RD_REGg_fifo_rptr;returnFAULT_NORMAL;}//清空整条轨迹缓存voidFifoClearAll(void){g_fifo_wptr0;g_fifo_rptr0;*(volatileuint8_t*)FIFO_WR_REG0;*(volatileuint8_t*)FIFO_RD_REG0;}四、新增 interpolate.c S型加减速直线插补底层源码#includearm_reg.h#includemath.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*pg_sc[axis];floatdtCTRL_PERIOD_MS/1000.0f;floatdv;switch(p-run_st){caseCURVE_ACC:p-accp-jerk*dt;if(p-accACC_MAX)p-run_stCURVE_CONST;break;caseCURVE_CONST://匀速段加速度不变break;caseCURVE_DEC:p-acc-p-jerk*dt;if(p-acc-ACC_MAX)p-run_stCURVE_IDLE;break;default:return0.0f;}dvp-acc*dt;p-vsdv;//速度限幅if(p-vsVEL_LIMIT)p-vsVEL_LIMIT;if(p-vs0)p-vs0;//本周期位移floatstepp-vs*dt;p-s_curstep;//行程走完进入空闲if(p-s_curp-s_total){p-run_stCURVE_IDLE;return0;}returnstep;}//初始化单轴S曲线参数voidSCurveInit(uint8_taxis,floattotal_dis,floatstart_v,floatend_v){SCurveParam*pg_sc[axis];p-s_totaltotal_dis;p-vsstart_v;p-veend_v;p-s_cur0.0f;p-jerkJERK_MAX;p-acc0;p-run_stCURVE_ACC;*(volatileuint8_t*)(S_CURVE_STAT_REGaxis)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_ti0;iAXIS_NUM;i){floatang_diff;switch(i){case0:ang_difftemp_ang.j1_ang-arm_dat-j1_ang;break;case1:ang_difftemp_ang.j2_ang-arm_dat-j2_ang;break;case2:ang_difftemp_ang.j3_ang-arm_dat-j3_ang;break;case3:ang_difftemp_ang.j4_ang-arm_dat-j4_ang;break;case4:ang_difftemp_ang.j5_ang-arm_dat-j5_ang;break;case5:ang_difftemp_ang.j6_ang-arm_dat-j6_ang;break;}//角度转脉冲角度/360*编码器单圈*减速比floatpulse_per_degENC_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_flag1;while(run_flag){run_flag0;for(uint8_ti0;iAXIS_NUM;i){floatstep_pulseSCurveCalcStep(i);int32_tdiraxis_dis[i]0?1:-1;int32_tsend_p(int32_t)(step_pulse*dir);if(g_sc[i].run_st!CURVE_IDLE)run_flag1;RawServoSend(i,send_p,5000);}}//更新当前关节角度*arm_dattemp_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写入点位进FIFO0x05启动FIFO整条轨迹运行case0x04:{//帧AA BB 04 px(4)py(4)pz(4)r1(4)r2(4)r3(4) CC DDCartPoint pt;pt.px*((float*)(buf3));pt.py*((float*)(buf7));pt.pz*((float*)(buf11));pt.r1*((float*)(buf15));pt.r2*((float*)(buf19));pt.r3*((float*)(buf23));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指令写入点位headbytes([0xAA,0xBB])cmdbytes([0x04])#float打包datastruct.pack(ffffff,x,y,z,r1,r2,r3)tailbytes([0xCC,0xDD])send_bufheadcmddatatail s.send(send_buf)returns.recv(64)defStartFifoTraj():#0x05启动整条轨迹bufbytes([0xAA,0xBB,0x05,0,0,0,0,0,0,0,0,0])bytes([0xCC,0xDD])s.send(buf)returns.recv(32)defClearFifo():#0x06清空FIFObufbytes([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闭环时序统一全寄存器直写无任何第三方运动控制库。