pid->handle

#include "Chassis_steer_function.h" #include "arm_math.h" #include "math.h" fp32 Chassis_Steer_PID_Calc(Chassis_steer_pid_t* pid, fp32 angle_ref, fp32 angle_fdb, fp32 speed_fdb) //底盘舵电机PID(角度+速度) { pid->angle_ref = angle_ref; pid->angle_fdb = angle_fdb; pid_calc(&pid->outer_pid, pid->angle_fdb, pid->angle_ref);//角度环 pid->speed_ref = pid->outer_pid.out; pid->speed_fdb = speed_fdb; pid_calc(&pid->inter_pid, pid->speed_fdb, pid->speed_ref);//速度环 return pid->inter_pid.out; } void Steer_Speed_Calculate(ChassisHandle_t* chassis_handle, fp32 chassis_vx, fp32 chassis_vy, fp32 chassis_vw) //舵轮轮速解算 { float theta = atan(1.0/1.0); //程序换算角度为弧度不能直接使用!! fp32 steer_vw=chassis_vw*3.14/180; float wheel_rpm_ratio; fp32 wheel_rpm[4]; fp32 max = 0; /*M3508_REDUCTION_RATIO为M3508减速比(1.0/19.0) */ wheel_rpm_ratio = 60.0f/(chassis_handle->structure.wheel_perimeter * M3508_REDUCTION_RATIO); //舵轮转速转换 wheel_rpm[0] = sqrt(pow(chassis_vx - steer_vw*RADIUS*sin(theta),2) + pow(chassis_vy - steer_vw*RADIUS*cos(theta),2))* wheel_rpm_ratio*chassis_handle->steer_set[0].speed_direction; //对应华南理工公式 Vx1-Vw1sin45*RADIUS wheel_rpm[1] = sqrt( pow(chassis_vx - steer_vw*RADIUS*sin(theta),2) + pow(chassis_vy + steer_vw*RADIUS*cos(theta),2))* wheel_rpm_ratio*chassis_handle->steer_set[1].speed_direction; //对应华南理工公式 Vx2+Vw2sin45*RADIUS wheel_rpm[2] = sqrt( pow(chassis_vx + steer_vw*RADIUS*sin(theta),2) + pow(chassis_vy + steer_vw*RADIUS*cos(theta),2))* wheel_rpm_ratio*chassis_handle->steer_set[2].speed_direction; //对应华南理工公式 Vy1-Vw1cos45*RADIUS wheel_rpm[3] = sqrt( pow(chassis_vx + steer_vw*RADIUS*sin(theta),2) + pow(chassis_vy - steer_vw*RADIUS*cos(theta),2))* wheel_rpm_ratio*chassis_handle->steer_set[3].speed_direction; //对应华南理工公式 Vy2+Vw2cos45*RADIUS //find max item for (uint8_t i = 0; i < 4; i++) { if(chassis_handle->turnFlag[i]==1) { wheel_rpm[i] = -wheel_rpm[i]; } else { wheel_rpm[i] = wheel_rpm[i]; } if (fabs(wheel_rpm[i]) > max) { max = fabs(wheel_rpm[i]); } } memcpy(chassis_handle->wheel_rpm, wheel_rpm, 4 * sizeof(fp32)); } void Steer_angle_change(ChassisHandle_t* chassis_handle, fp32 chassis_vx, fp32 chassis_vy, fp32 chassis_vw) { float theta = atan(1.0/1.0); fp32 wheel_angle[4]; fp32 steer_vw=chassis_vw*3.14/180; if((chassis_vx==0)&&(chassis_vy==0)&&(chassis_vw==0)) { memcpy(wheel_angle, chassis_handle->lastSteeringAngletarget, 4 * sizeof(fp32)); } else //舵轮角度解算 { wheel_angle[0]=atan2((chassis_vy-steer_vw*RADIUS*sin(theta)), (chassis_vx-steer_vw*RADIUS*cos(theta))); //对应华南理工公式VX=tan-1((vy1-vw1cos45)/(vx1-vw1sin45)) wheel_angle[1]=atan2((chassis_vy+steer_vw*RADIUS*sin(theta)), (chassis_vx-steer_vw*RADIUS*cos(theta))); //对应华南理工公式VX=tan-1((vy2-vw2cos45)/(vx2+vw2sin45)) wheel_angle[2]=atan2((chassis_vy+steer_vw*RADIUS*sin(theta)), (chassis_vx+steer_vw*RADIUS*cos(theta))); //对应华南理工公式VX=tan-1((vy3+vw3cos45)/(vx3+vw3sin45)) wheel_angle[3]=atan2((chassis_vy-steer_vw*RADIUS*sin(theta)), (chassis_vx+steer_vw*RADIUS*cos(theta))); //对应华南理工公式VX=tan-1((vy4+vw4cos45)/(vx4-vw4sin45)) for(uint8_t i=0;i<4;i++) { if(wheel_angle[i]-chassis_handle->lastSteeringAngletarget[i]>PI/2) { wheel_angle[i]=fmodf(wheel_angle[i]-PI,2*PI); chassis_handle->turnFlag[i]=1; } else if(wheel_angle[i]-chassis_handle->lastSteeringAngletarget[i]<-PI/2) { wheel_angle[i]=fmodf(wheel_angle[i]+PI,2*PI); chassis_handle->turnFlag[i]=1; } else { chassis_handle->turnFlag[i]=0; } } } memcpy(chassis_handle->lastSteeringAngletarget, wheel_angle, 4 * sizeof(fp32)); for(uint8_t i=0;i<4;i++) { wheel_angle[i]=wheel_angle[i]*180/PI; } memcpy(chassis_handle->steeringAngleTarget, wheel_angle, 4 * sizeof(fp32)); } void Steer_Calculate(ChassisHandle_t* chassis_handle, fp32 chassis_vx, fp32 chassis_vy, fp32 chassis_vw) //舵轮底盘解算 { Steer_angle_change(chassis_handle,chassis_vx,chassis_vy,chassis_vw); Steer_Speed_Calculate(chassis_handle,chassis_vx,chassis_vy,chassis_vw); } void Steer_Chassis_ControlCalc(ChassisHandle_t* chassis_handle) { Steer_Calculate(chassis_handle, chassis_handle->vx, chassis_handle->vy, chassis_handle->vw); } void calculateRoundCnt(ChassisHandle_t* chassis_handle) { static float last_encoder[4] = {0}; for(uint8_t i=0;i<4;i++) { float now_encoder = chassis_handle->chassis_steer_motor[i].motor_info->ecd - chassis_handle->chassis_steer_motor[i].offset_ecd.chassis_offset_ecd; now_encoder = now_encoder/8192*360; if(now_encoder - last_encoder[i] > 180) chassis_handle->motor_circle[i] --; else if(now_encoder - last_encoder[i] < -180) chassis_handle->motor_circle[i] ++; last_encoder[i] = now_encoder; chassis_handle->steeringAngle[i] = -now_encoder - chassis_handle->motor_circle[i]*360; chassis_handle->chassis_steer_motor[i].sensor.relative_angle=chassis_handle->steeringAngle[i]*-1; } } void calculateTargetRoundCnt(ChassisHandle_t* chassis_handle) { float now_target[4]; static float last_target[4]={0,0,0,0}; memcpy(now_target, chassis_handle->steeringAngleTarget, 4 * sizeof(fp32)); for(uint8_t i=0;i<4;i++) { if(now_target[i] - last_target[i] > 180) chassis_handle->motor_target_count[i] --; else if(now_target[i] - last_target[i] < -180) chassis_handle->motor_target_count[i] ++; last_target[i] = now_target[i]; chassis_handle->steeringAngleTarget[i] = now_target[i] + chassis_handle->motor_target_count[i]*360 ; } }加上注释
06-19
物联网通信协议测试是保障各类设备间实现可靠数据交互的核心环节。在众多适用于物联网的通信协议中,MQTT(消息队列遥测传输)以其设计简洁与低能耗的优势,获得了广泛应用。为确保MQTT客户端与服务端的实现严格遵循既定标准,并具备良好的互操作性,实施系统化的测试验证至关重要。 为此,采用TTCN-3(树表结合表示法第3版)这一国际标准化测试语言构建的自动化测试框架被引入。该语言擅长表达复杂的测试逻辑与数据结构,同时保持了代码的清晰度与可维护性。基于此框架开发的MQTT协议一致性验证套件,旨在自动化地检验MQTT实现是否完全符合协议规范,并验证其与Eclipse基金会及欧洲电信标准化协会(ETSI)所发布的相关标准的兼容性。这两个组织在物联网通信领域具有广泛影响力,其标准常被视为行业重要参考。 MQTT协议本身存在多个迭代版本,例如3.1、3.1.1以及功能更为丰富的5.0版。一套完备的测试工具必须能够覆盖对这些不同版本的验证,以确保基于各版本开发的设备与应用均能满足一致的质量与可靠性要求,这对于物联网生态的长期稳定运行具有基础性意义。 本资源包内包含核心测试框架文件、一份概述性介绍文档以及一份附加资源文档。这些材料共同提供了关于测试套件功能、应用方法及可能包含的扩展工具或示例的详细信息,旨在协助用户快速理解并部署该测试解决方案。 综上所述,一个基于TTCN-3的高效自动化测试框架,为执行全面、标准的MQTT协议一致性验证提供了理想的技术路径。通过此类专业测试套件,开发人员能够有效确保其MQTT实现的规范符合性与系统兼容性,从而为构建稳定、安全的物联网通信环境奠定坚实基础。 资源来源于网络分享,仅用于学习交流使用,请勿用于商业,如有侵权请联系我删除!
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值