liumin/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Compare commits

base: liumin:转速模式
Branches Tags
liumin:main liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序
...
compare: liumin:20251022-7-8号车程序
Branches Tags
liumin:7-8car-chassis liumin:Pan-Tilt-Unit liumin:main liumin:gimbal liumin:6-car liumin:1car liumin:4while4turn liumin:转速模式
liumin:20251022-7-8号车程序

3 Commits
转速模式 ... 20251022-7

Author SHA1 Message Date
liumin
28e3430e60 增加所有参数注释 2025-07-30 14:55:50 +08:00
liumin
febaad3dc6 修改单个参数读取 2025-07-30 11:28:53 +08:00
liumin
242dcb6feb 扭矩模式修改限制 2025-07-25 21:35:22 +08:00
2 changed files with 524 additions and 484 deletions
Expand all files Collapse all files

41
app/app_differential_drive.c
View File

@@ -427,10 +427,14 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
float left_speed_mps = 0.0f; float left_speed_mps = 0.0f;
float right_speed_mps = 0.0f; float right_speed_mps = 0.0f;
float max_torque = diff_data.max_Torq; float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了
// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); {
linear_velocity_x = 0;
}
left_speed_mps = linear_velocity_x + yaw_rate; left_speed_mps = linear_velocity_x + yaw_rate;
right_speed_mps = linear_velocity_x - yaw_rate; right_speed_mps = linear_velocity_x - yaw_rate;
@@ -474,6 +478,7 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]);
handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]);
// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq); // distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq); // distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);
@@ -608,7 +613,7 @@ static void diffProcess(void *signal_id)
// 限制输出速度在当前速度和最大加速度计算出来的速度之间 // 限制输出速度在当前速度和最大加速度计算出来的速度之间
// output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_speed) ) )//手柄回中，速度小的时候清0
{ {
resetPidIntegral(&speed_pid); resetPidIntegral(&speed_pid);
resetPidIntegral(&yaw_rate_pid); resetPidIntegral(&yaw_rate_pid);
@@ -622,7 +627,8 @@ static void diffProcess(void *signal_id)
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque);
if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍
if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
{ {
diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed;
diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID
@@ -635,7 +641,7 @@ static void diffProcess(void *signal_id)
} }
if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 if( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数
{ {
diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed;
diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID
@@ -647,32 +653,37 @@ static void diffProcess(void *signal_id)
diff_data.right_diff_touue = 0; diff_data.right_diff_touue = 0;
} }
if(out_torque[0] > 0)
if(out_torque[0] > 0)//根据大小来限定值为分配扭矩。最小就是0扭矩。
{ {
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]); diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]);
} }
else else
{ {
diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]); diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]);
} }
if(out_torque[1] > 0) if(out_torque[1] > 0)
{ {
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]); diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]);
} }
else else
{ {
diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]); diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]);
} }
diff_data.out_torq[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue);
diff_data.out_torq[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f; diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue);
diff_data.out_torq[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f; diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue);
out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩
out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq);
out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq);
out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq);
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); // printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);

105
app/app_param_manage.c
View File

@@ -278,21 +278,31 @@ void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip,
void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) { void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
// 处理读请求的逻辑
printf("Processing read request.\n"); printf("Processing read request.\n");
// 清零 paramRequest // 先备份原始请求数据
UnParamRequest originalRequest;
memcpy(&originalRequest, paramRequest, sizeof(UnParamRequest));
// 清零响应数据
memset(paramRequest, 0, sizeof(UnParamRequest)); memset(paramRequest, 0, sizeof(UnParamRequest));
// 先发送信号，然后从结构体读数 // 处理客户端请求的参数
for (int i = 0; i < 256; ++i) { for (int i = 0; i < 256; ++i) {
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) { if (strlen((char *)originalRequest.bit_data.param_name[i]) > 0) {
float readData = readParameter(paramRequest->bit_data.param_name[i]); // 复制参数名到响应
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(paramRequest->bit_data.data[i])); strcpy((char *)paramRequest->bit_data.param_name[i],
(char *)originalRequest.bit_data.param_name[i]);
// 读取参数值
float readData = readParameter(originalRequest.bit_data.param_name[i]);
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(float));
printf("Read parameter: %s = %f\n",
originalRequest.bit_data.param_name[i], readData);
} }
} }
// 发送响应 // 发送响应 - 直接传递 paramRequest
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0); sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
} }
@@ -300,6 +310,7 @@ void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, u
void OnParamSignal(void *data) void OnParamSignal(void *data)
{ {
RequestContext *signal = (RequestContext *)data; RequestContext *signal = (RequestContext *)data;
@@ -421,37 +432,55 @@ void paramAppInit(void)
// 上电读取所有参数 // 上电读取所有参数
memset(param_manager.arr, 0, sizeof(param_manager.arr)); memset(param_manager.arr, 0, sizeof(param_manager.arr));
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION); accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
// whl_bas 轮胎直径
// 初始化每个参数 // max_rpm 最大转速
// param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距 // whl_dia 轴距
// param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速 // max_acc 最大加速度
// param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径 // spd_kp 速度P
// param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度 // spd_ki 速度I
// param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP // spd_kd 速度D
// param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI // spd_il 速度积分限制
// param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD // spd_ol 速度PID输出限制
// param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL // crv_kp 转弯P
// param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL // crv_ki 转弯I
// param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP // crv_kd 转弯D
// param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI // crv_il 转弯积分限制
// param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD // crv_ol 转弯PID输出限制
// param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL // brk_on 刹车刹紧时间
// param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL // brk_off 刹车释放时间
// param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数 // maxTorq 最大扭矩
// param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数 // feedPwr 最大馈电功率
// param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩 // dispPwr 最大放电功率
// param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率 // VehMass 车重
// param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率 // gRatio 减速比
// param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量 // prCTime 预充电时间
// param_manager.bit_data.gRatio = 28.0f; // 初始化减速比 // brk_pos 刹车位置
// param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间 // pwr_sta 电源状态
// param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车 // high_sw 高压开关状态
// param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态 // stop_sw 急停开关状态
// param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态 // lightSt 灯光状态
// param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态 // pwr_btn 电源开关状态
// param_manager.bit_data.test = 0.0f; // 初始化测试参数 // sleepTm 休眠时间
// wakeTm 唤醒时间
// Ospd_kp 自主速度P
// Ospd_ki 自主速度I
// Ospd_kd 自主速度D
// Ospd_il 自主速度积分限制
// Ospd_ol 自主速度PID输出限制
// Ocrv_kp 自主转弯P
// Ocrv_ki 自主转弯I
// Ocrv_kd 自主转弯D
// Ocrv_il 自主转弯积分限制
// Ocrv_ol 自主转弯PID输出限制
// minTorq 输出扭矩死区
// brk_rev 刹车方向
// mot_kp 同侧扭矩P参数
// mot_ki 同侧扭矩I参数
// mot_kd 同侧扭矩d参数
// mot_il 同侧扭矩积分限制
// mot_ol 同侧扭矩输出限制
// diff_sp 同侧扭矩速度差阈值
// test 初始化测试参数
// 订阅信号 // 订阅信号
subscribe(&param_signal, handleParamOp); subscribe(&param_signal, handleParamOp);
subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号 subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号