diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index a65ec83..2e5c565 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -1,934 +1,934 @@ -#include "app_config.h" -#include "interface.h" - -#include "app_frm_monitor.h" -#include "app_frm_signal.h" -#include "app_frm_timer.h" -#include "app_param_manage.h" -#include "app_pid.h" - -#include "app_differential_drive.h" -#include "app_brake.h" -#include "app_power.h" - -Timer diff_app_timer; - -// 定义全局变量 -DiffData diff_data; - -PID_t speed_pid; -PID_t yaw_rate_pid; - -// 设置电机输出 -void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) -{ - - float abs_left_front_speed = 0; - float abs_right_front_speed = 0; - float abs_left_rear_speed = 0; - float abs_right_rear_speed = 0; - - // 档位 - un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡 - un_motor_output2.bit_data.gear = diff_data.state; - un_motor_output3.bit_data.gear = diff_data.state; - un_motor_output4.bit_data.gear = diff_data.state; - - - //增加系数以及偏移量 - if(diff_data.state == STATE_FORWARD)//根据挡位来判断，扭矩的正负 - { - abs_left_front_speed = (out_torq[0] + 300.0f) *100.0f; - abs_right_front_speed = (out_torq[1] + 300.0f) *100.0f; - abs_left_rear_speed = (out_torq[2] + 300.0f) *100.0f; - abs_right_rear_speed = (out_torq[3] + 300.0f) *100.0f; - } - else if(diff_data.state == STATE_BACKWARD)//倒挡直接修改为负扭矩发送出去 - { - abs_left_front_speed = (-out_torq[0] + 300.0f) *100.0f; - abs_right_front_speed = (-out_torq[1] + 300.0f) *100.0f; - abs_left_rear_speed = (-out_torq[2] + 300.0f) *100.0f; - abs_right_rear_speed = (-out_torq[3] + 300.0f) *100.0f; - } - else//空挡直接发0 - { - abs_left_front_speed = 0; - abs_right_front_speed = 0; - abs_left_rear_speed = 0; - abs_right_rear_speed = 0; - } - - - // 设置左右电机期望转速 -// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000 -// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000 - - // 设置模式为扭矩模式 - un_motor_output1.bit_data.mode = MOTOR_MODE; - un_motor_output2.bit_data.mode = MOTOR_MODE; - un_motor_output3.bit_data.mode = MOTOR_MODE; - un_motor_output4.bit_data.mode = MOTOR_MODE; - - // 设置最大扭矩 - un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); - un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed ); - un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed ); - un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); - - - // 设置馈电功率 - un_motor_output1.bit_data.feed_power = feed_power; - un_motor_output2.bit_data.feed_power = feed_power; - un_motor_output3.bit_data.feed_power = feed_power; - un_motor_output4.bit_data.feed_power = feed_power; - - - // 设置放电功率 - un_motor_output1.bit_data.discharge_power = discharge_power; - un_motor_output2.bit_data.discharge_power = discharge_power; - un_motor_output3.bit_data.discharge_power = discharge_power; - un_motor_output4.bit_data.discharge_power = discharge_power; - - -} - - -// 限制值在最小值和最大值之间 -float constrain(float value, float min_val, float max_val) -{ - if (value < min_val) - { - return min_val; - } - else if (value > max_val) - { - return max_val; - } - else - { - return value; - } -} - -// 计算当前速度、角速度 -uint8_t calculateCurrentSpeedYawRate(void) -{ - // 获取轮子周长 - float wheel_circumference = (float)getParam("whl_dia") * M_PI; - // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); - if (fabsf(gear_ratio) < EPSILON) - { - return 0; // 避免除以0的情况 - } - // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 - - - float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio); - float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio); - -// float left_speed_mps = 0; -// float right_speed_mps = 0; - -// float left_speed_mps = 0; -// float right_speed_mps = 0; - - // 计算当前速度 - diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; - // 计算速度差 - float speed_diff = left_speed_mps - right_speed_mps; - // 计算角速度 - float wheel_base = (float)getParam("whl_bas"); - if (fabsf(wheel_base) < EPSILON) - { - return 0; // 避免除以0的情况 - } - diff_data.yaw_rate = speed_diff / wheel_base; - - return 0; -} - - -// 计算加速度 -float calculateAcceleration(float speed, float previous_speed, float dt) -{ - if (fabs(dt) < EPSILON) - { - return 0; // 避免除以0的情况 - } - float acceleration = (speed - previous_speed) / dt; - return acceleration; -} - -// 计算减速度 -float calculateDeceleration(float speed, float previous_speed, float dt) -{ - if (fabs(dt) < EPSILON) - { - return 0; // 避免除以0的情况 - } - float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度 - return deceleration; -} - -// 计算最大速度 -float calculateMaxSpeed() -{ - // 获取最大电机转速 (RPM) - float max_rpm = (float)getParam("max_rpm"); - // 获取轮子周长 - float wheel_circumference = (float)getParam("whl_dia") * M_PI; - // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); - if (fabsf(gear_ratio) < EPSILON) - { - return 0; // 避免除以0的情况 - } - // 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 - float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio); - return max_speed; -} - -// 计算最大加速度 -float calculateMaxAcceleration(void) -{ - // 获取车辆参数 - float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩 - float vehicle_mass = (float)getParam("VehMass"); // 车辆质量 - float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径 - float gear_ratio = (float)getParam("gRatio"); // 减速比 - if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON ) - { - return 0; // 避免除以0的情况 - } - // 减速比计算扭矩 - float effective_torque = max_motor_torque * gear_ratio; - // 计算最大加速度 - float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass; - return max_acceleration; -} - -// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 -void calculateCurrentState(float dt) -{ - static float previous_speed = 0.0f; - // 更新当前速度和当前角速度 - calculateCurrentSpeedYawRate(); - // 更新加速度、减速度等，根据需要计算 - diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt); - diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt); - diff_data.max_speed = calculateMaxSpeed(); - previous_speed = diff_data.speed; -} - - -/** - * @brief 基于转速反比的双电机扭矩分配函数 - * @param rpm1 电机1当前转速（单位：rpm） - * @param rpm2 电机2当前转速（单位：rpm） - * @param total_torque 系统总需求扭矩（单位：Nm） - * @param torque1 [out] 电机1分配到的扭矩（单位：Nm） - * @param torque2 [out] 电机2分配到的扭矩（单位：Nm） - * @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡 - */ -void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque) -{ - - // 总扭矩为0时快速返回 - if (fabs(total_torque) < 0.001f) { - *torque1 = 0.0f; - *torque2 = 0.0f; - return; - } - -// // 保护条件：当两电机均静止时采用平均分配策略 -// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) { -// *torque1 = total_torque / 2.0f; -// *torque2 = total_torque / 2.0f; -// return; -// } - - // 计算权重因子（与转速成反比关系） - // 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理 - float weight1 = 1.0f / (fabs(rpm1) + 0.001f); - float weight2 = 1.0f / (fabs(rpm2) + 0.001f); - - // 归一化计算分配比例 - float total_weight = weight1 + weight2; - *torque1 = total_torque * (weight1 / total_weight); - *torque2 = total_torque * (weight2 / total_weight); - - // 独立限制单侧扭矩（修改核心逻辑） - if (fabs(*torque1) > max_torque) { - *torque1 = copysignf(max_torque, *torque1); - } - if (fabs(*torque2) > max_torque) { - *torque2 = copysignf(max_torque, *torque2); - } - - // 仅对非零扭矩应用下限限制 - if (fabs(*torque1) < min_torque) { - *torque1 = copysignf(min_torque, *torque1); - } - if ( fabs(*torque2) < min_torque) { - *torque2 = copysignf(min_torque, *torque2); - } - -} - - - -/** - * @brief 根据轮速差动态调整电机扭矩（带非负限制） - * @param speed_left 左轮速度（单位：rpm或自定义） - * @param speed_right 右轮速度（单位：rpm或自定义） - * @param torque_left 左轮扭矩指针（单位：Nm或自定义） - * @param torque_right 右轮扭矩指针（单位：Nm或自定义） - * @param threshold 触发调整的速差阈值（单位同轮速） - * @param k 扭矩调整系数（无量纲，建议0 threshold) { - // 计算需要减少的扭矩量（速差超出阈值部分×系数） - float torque_reduction = (speed_diff - threshold) * k; - - if (speed_left > speed_right) { - // 左轮过快时减少左扭矩，并限制最小值为0 - *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f); - } else { - // 右轮过快时减少右扭矩，并限制最小值为0 - *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f); - } - } -} -// 计算左右电机速度 -void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed) -{ - // 防止速度过低导致不必要的计算 - if (fabs(max_speed) < EPSILON) - { - motor_speed[0] = 0.0f; - motor_speed[1] = 0.0f; - motor_speed[2] = 0.0f; - motor_speed[3] = 0.0f; - return; - } - -#if THROTTLE_PID_MODE - float max_torque = (float)getParam("maxTorq"); - - linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); - yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); - - - float left_speed_mps = linear_velocity_x + yaw_rate; - float right_speed_mps = linear_velocity_x - yaw_rate; - -//扭矩分配 - if(max_torque < left_speed_mps) - { - right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 - left_speed_mps = max_torque; - } - else if(-max_torque > left_speed_mps) - { - right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 - left_speed_mps = -max_torque; - } - else if(max_torque < right_speed_mps) - { - left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 - right_speed_mps = max_torque; - } - else if(-max_torque > right_speed_mps) - { - left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 - right_speed_mps = -max_torque; - } - else{} - - printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate); - - motor_speed[0] = left_speed_mps; - motor_speed[2] = left_speed_mps; - - motor_speed[1] = right_speed_mps; - motor_speed[3] = right_speed_mps; - - adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5); - adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5); - - -// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); - - - - - - - - - -// 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); - -// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]); - -// // 返回计算结果 -// *left_motor_speed = left_speed_mps; -// *right_motor_speed = right_speed_mps; -#else - - // 限制线速度和偏航率 - linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); - float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); - yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate); - - // 计算旋转速度 - float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate; - - // 计算车辆左右线速度 (m/s) - float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换 - float right_speed_mps = linear_velocity_x + rotational_velocity; - - // 计算轮子周长 - float wheel_circumference = (float)getParam("whl_dia") * M_PI; - - // 将车辆左右线速度转换为轮子转速 (RPM) - float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference; - float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference; - - // 获取减速比 - float gear_ratio = (float)getParam("gRatio"); - - // 将轮子转速转换为电机转速，考虑减速比 - float left_motor_rpm = left_wheel_rpm * gear_ratio; - float right_motor_rpm = right_wheel_rpm * gear_ratio; - - // 限制电机的最大和最小转速 - float max_motor_rpm = (float)getParam("max_rpm"); - left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm); - right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm); - // 当电机转速小于50转时，设置为0 - if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转 - { - left_motor_rpm = 0; - } - if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转 - { - right_motor_rpm = 0; - } - // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下 - // left_motor_rpm = -left_motor_rpm; - // 返回计算结果 - *left_motor_speed = left_motor_rpm; - *right_motor_speed = right_motor_rpm; - -#endif -} - -// 映射遥控器速度，分为死区、低速区和高速区。 -float mapRemoteControlSpeed( - float input_speed, - float deadzone_limit, - float input_max, - float output_max, - float input_slow, - float output_slow -) -{ - float output_speed = 0.0f; - // 获取输入速度的绝对值 - float abs_input = fabsf(input_speed); -//diff_data.desired_speed, 0.1, 2, 10, 1, 5 - -// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); - - if (abs_input < deadzone_limit + EPSILON) - { - output_speed = 0.0f;// 死区 - } - else if (abs_input < input_slow + EPSILON)// 低速区 - { - output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit);// - } - else if (abs_input <= input_max + EPSILON)// 高速区 - { - output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow);// 0.2 + （3 - 0.5）* （15-0.2） / （17 - 0.5） - } - else // 超出范围 - { - output_speed = output_max; - } - - // 根据原始输入速度的符号恢复方向 - if (input_speed < 0) - { - output_speed = -output_speed; - } - return output_speed; -} - -/** - * @brief 状态机处理函数（修改后版本） - */ -void handleVehicleState(DiffData *ctx) -{ - switch (ctx->state) - { - //------------------------------------------- - // 初始状态：根据期望速度方向跳转 - //------------------------------------------- - case STATE_INIT: - { - if (ctx->desired_speed < 0.0f) - { - ctx->state = STATE_BACKWARD; - } - else - { - ctx->state = STATE_FORWARD; - } - break; - } - - //------------------------------------------- - // 前进状态：处理反向指令（新增else分支） - //------------------------------------------- - case STATE_FORWARD: - { - if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f)) - { - ctx->state = STATE_BACKWARD; // 零速时允许切换方向 - } - else if ((ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f)) - { - ctx->desired_speed = 0.0f; // 非零速时清空期望速度 - ctx->state = STATE_FORWARD; // 显式保持当前状态 - } - else - { - ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态 - } - break; - } - - //------------------------------------------- - // 倒车状态：处理正向指令（新增else分支） - //------------------------------------------- - case STATE_BACKWARD: - { - if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) - { - ctx->state = STATE_FORWARD; // 零速时允许切换方向 - } - else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f)) - { - ctx->desired_speed = 0.0f; // 非零速时清空期望速度 - ctx->state = STATE_BACKWARD; // 显式保持当前状态 - } - else - { - ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态 - } - break; - } - } -} - - - - -// 差速处理函数 -static void diffProcess(void *signal_id) -{ - (void)signal_id; - static float previous_time1 = 0.0f; - - float time1 = (float)getCurrentTime(); - float dt = (time1 - previous_time1) / PERIOD_TICK; - previous_time1 = time1; - - // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 - calculateCurrentState(dt); - - // 当速度小于1时，设定为原地转向 20250321 修改为考虑负号 - if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) ) - { - diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f; - } - else if( (diff_data.desired_speed < 0) && (diff_data.desired_speed >= -1.0f) ) - { - diff_data.desired_yaw_rate = diff_data.desired_curvature * -1.0f; - } - else - { - diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed; - } - - handleVehicleState(&diff_data); //20250704 换挡函数 速度为0才能换挡 - - -// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); - // 使用 PID 控制器计算输出速度和曲率 - float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); - float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); - - // 计算最大加速度，用函数计算 - float max_acceleration = calculateMaxAcceleration(); - // 限制输出速度在当前速度和最大加速度计算出来的速度之间 - // 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 - { - resetPidIntegral(&speed_pid); - resetPidIntegral(&yaw_rate_pid); - output_speed = 0; - output_yaw_rate = 0; - } - -// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); - - -// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下 -// { -// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。 -// { -// output_yaw_rate = 500; -// } -// } - - - // 使用差速车辆动力学模型计算左右电机的期望速度 - computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]); - - - - - - - -// if( (left_speed < 200) && (left_speed > -200) ) -// { -// left_speed = 0; -// } -// -// if( (right_speed < 200) && (right_speed > -200) ) -// { -// right_speed = 0; -// } - - - // 设置电机输出 - setMotorOutput(&diff_data.out_torq[0], - diff_data.max_Torq,// - (uint16_t)getParam("feedPwr"), - (uint16_t)getParam("dispPwr")); - // 发布左右电机期望转速,电源在工作状态才能发送 - if (power_data.current_state == POWER_WORKING) - { - publishMessage(&un_motor_output1, 1); - publishMessage(&un_motor_output2, 1); - publishMessage(&un_motor_output3, 1); - publishMessage(&un_motor_output4, 1); - - } - - - un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10); - un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10); - - un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); - un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); - - un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed); - un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed); - - publishMessage(&diff_data, 1); - -} - -/****************************************************************************** - Filter(); N个数中取两个 -******************************************************************************/ -int16_t Filter(int16_t *s,uint8_t Len) -{ - uint8_t i,j; - int16_t temp; -//降序排序 - for(i=0;i*(s+j)) - { - *(s+i)=*(s+i)^*(s+j); - *(s+j)=*(s+j)^*(s+i); - *(s+i)=*(s+i)^*(s+j); - } - } - temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移 - return(temp); -} - - - - - - -// 差速输入处理函数 -static void diffInput(void *signal_id) -{ - float motor_speed_temp = 0.0f; - - if (signal_id == &un_sw_sample) - { - diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; - } - else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 - { - diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b; - diff_data.mode = un_remote_control_input.bit_data.switch_c == 1 ? MODE_AUTO : MODE_MANUAL; - - if (diff_data.mode == MODE_MANUAL) - { - diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); - diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); - // 单位转换 - diff_data.desired_speed = diff_data.desired_speed * 0.01f; - diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; - // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 - diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); - diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5); - - if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题 - { - diff_data.desired_curvature = diff_data.desired_curvature; - } - else - { - diff_data.desired_curvature = -diff_data.desired_curvature; - } - } - } - else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) ) - { - diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); - diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); - // 单位转换 - diff_data.desired_speed = diff_data.desired_speed * 0.01f; - diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; - // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 - diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题 - diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); - } - else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) - { - diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed)); - diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature)); - // 单位转换 - diff_data.desired_speed = diff_data.desired_speed * 0.01f; - diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相 - // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 - diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5); - diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); - } - else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机） - { - diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量 - diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量 - - if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.left_front_motor_speed; - } - else - { - motor_speed_temp = diff_data.left_rear_motor_speed; - } - diff_data.left_motor_speed = motor_speed_temp; - } - else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机） - { - diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相 - diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000)); - - if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 - { - motor_speed_temp = diff_data.right_rear_motor_speed; - } - else - { - motor_speed_temp = diff_data.right_front_motor_speed; - } - - diff_data.right_motor_speed = motor_speed_temp; - } - - // 急停开关 - diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); - - // 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关 - if (diff_data.emergency_stop_state == 1) - { - diff_data.desired_speed = 0.0; - diff_data.desired_curvature = 0.0; - } - // 遥控器断线，而且是在手动模式，期望值清0 - if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) ) - { - diff_data.desired_speed = 0.0; - diff_data.desired_curvature = 0.0; - } - - if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 - { - diff_data.max_Torq = 5;//20240403修改。刹车就是5N - } - else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩 - { - diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题 - } - else - { - diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 - } - - diffProcess(&diff_data);//计算左右电机期望转速 -} - - - -// 预充完成处理函数 -void preChargeFinish(void *signal_id) -{ - (void)signal_id; - - float out_torq[4] = {0.0f,0.0f,0.0f,0.0f}; - - setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); - // 档位 -// un_motor_output1.bit_data.gear = 0; // 0表示空挡 -// un_motor_output2.bit_data.gear = 0; - publishMessage(&un_motor_output1, 1); - publishMessage(&un_motor_output2, 1); - publishMessage(&un_motor_output3, 1); - publishMessage(&un_motor_output4, 1); -} - - -void diffParametersInit(void *signal_id) -{ - (void)signal_id; // 标记变量为已使用，避免编译器警告 - - if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID - { - setPidParameters(&speed_pid, - getParam("Ospd_kp"), - getParam("Ospd_ki"), - getParam("Ospd_kd"), - getParam("Ospd_il"), - getParam("Ospd_ol") - ); - - setPidParameters(&yaw_rate_pid, - getParam("Ocrv_kp"), - getParam("Ocrv_ki"), - getParam("Ocrv_kd"), - getParam("Ocrv_il"), - getParam("Ocrv_ol") - ); - } - else//手动模式 - { - setPidParameters(&speed_pid, - getParam("spd_kp"), - getParam("spd_ki"), - getParam("spd_kd"), - getParam("spd_il"), - getParam("spd_ol") - ); - - setPidParameters(&yaw_rate_pid, - getParam("crv_kp"), - getParam("crv_ki"), - getParam("crv_kd"), - getParam("crv_il"), - getParam("crv_ol") - ); - } - diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 - - - printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); - printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); -// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); - - printf("left_motor_speed = %f\n",diff_data.left_motor_speed); - printf("right_motor_speed = %f\n",diff_data.right_motor_speed); - -// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); -// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]); - - float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); - float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); - // 单位转换 - deffspeed = deffspeed * 0.01f; - deffcurvature = deffcurvature * 0.0001f; - - printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature); - - timerStart(&diff_app_timer,1000,1);//1s调用一次 -} - - -// 差速初始化函数 -void diffAppInit(void) -{ - // 初始化 diff_data - memset(&diff_data, 0, sizeof(DiffData)); - - // 订阅相关信号 - subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关 - subscribe(&un_motor_input1, diffInput); - subscribe(&un_motor_input2, diffInput); - - - subscribe(&un_auto_computer_input, diffInput); - subscribe(&un_manual_computer_input, diffInput); - subscribe(&un_remote_control_input, diffInput); - subscribe(&power_data.pre_charge_finish, preChargeFinish); - - // 初始化速度 PID 控制器 - initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); - // 设置速度 PID 控制器的参数 - setPidParameters(&speed_pid, - getParam("spd_kp"), - getParam("spd_ki"), - getParam("spd_kd"), - getParam("spd_il"), - getParam("spd_ol") - ); - - // 初始化曲率 PID 控制器 - initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); - // 设置曲率 PID 控制器的参数 - setPidParameters(&yaw_rate_pid, - getParam("crv_kp"), - getParam("crv_ki"), - getParam("crv_kd"), - getParam("crv_il"), - getParam("crv_ol") - ); - - subscribe(&diff_app_timer, diffParametersInit); - timerStart(&diff_app_timer,1000,1);//1s调用一次 - - printf("diffControl: diffAppInit OK \n"); -} +#include "app_config.h" +#include "interface.h" + +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" +#include "app_pid.h" + +#include "app_differential_drive.h" +#include "app_brake.h" +#include "app_power.h" + +Timer diff_app_timer; + +// 定义全局变量 +DiffData diff_data; + +PID_t speed_pid; +PID_t yaw_rate_pid; + +// 设置电机输出 +void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) +{ + + float abs_left_front_speed = 0; + float abs_right_front_speed = 0; + float abs_left_rear_speed = 0; + float abs_right_rear_speed = 0; + + // 档位 + un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡 + un_motor_output2.bit_data.gear = diff_data.state; + un_motor_output3.bit_data.gear = diff_data.state; + un_motor_output4.bit_data.gear = diff_data.state; + + + //增加系数以及偏移量 + if(diff_data.state == STATE_FORWARD)//根据挡位来判断，扭矩的正负 + { + abs_left_front_speed = (out_torq[0] + 300.0f) *100.0f; + abs_right_front_speed = (out_torq[1] + 300.0f) *100.0f; + abs_left_rear_speed = (out_torq[2] + 300.0f) *100.0f; + abs_right_rear_speed = (out_torq[3] + 300.0f) *100.0f; + } + else if(diff_data.state == STATE_BACKWARD)//倒挡直接修改为负扭矩发送出去 + { + abs_left_front_speed = (-out_torq[0] + 300.0f) *100.0f; + abs_right_front_speed = (-out_torq[1] + 300.0f) *100.0f; + abs_left_rear_speed = (-out_torq[2] + 300.0f) *100.0f; + abs_right_rear_speed = (-out_torq[3] + 300.0f) *100.0f; + } + else//空挡直接发0 + { + abs_left_front_speed = 0; + abs_right_front_speed = 0; + abs_left_rear_speed = 0; + abs_right_rear_speed = 0; + } + + + // 设置左右电机期望转速 +// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000 +// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000 + + // 设置模式为扭矩模式 + un_motor_output1.bit_data.mode = MOTOR_MODE; + un_motor_output2.bit_data.mode = MOTOR_MODE; + un_motor_output3.bit_data.mode = MOTOR_MODE; + un_motor_output4.bit_data.mode = MOTOR_MODE; + + // 设置最大扭矩 + un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed ); + un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed ); + un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed ); + un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed ); + + + // 设置馈电功率 + un_motor_output1.bit_data.feed_power = feed_power; + un_motor_output2.bit_data.feed_power = feed_power; + un_motor_output3.bit_data.feed_power = feed_power; + un_motor_output4.bit_data.feed_power = feed_power; + + + // 设置放电功率 + un_motor_output1.bit_data.discharge_power = discharge_power; + un_motor_output2.bit_data.discharge_power = discharge_power; + un_motor_output3.bit_data.discharge_power = discharge_power; + un_motor_output4.bit_data.discharge_power = discharge_power; + + +} + + +// 限制值在最小值和最大值之间 +float constrain(float value, float min_val, float max_val) +{ + if (value < min_val) + { + return min_val; + } + else if (value > max_val) + { + return max_val; + } + else + { + return value; + } +} + +// 计算当前速度、角速度 +uint8_t calculateCurrentSpeedYawRate(void) +{ + // 获取轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + // 获取减速比 + float gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 + + + float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio); + +// float left_speed_mps = 0; +// float right_speed_mps = 0; + +// float left_speed_mps = 0; +// float right_speed_mps = 0; + + // 计算当前速度 + diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f; + // 计算速度差 + float speed_diff = left_speed_mps - right_speed_mps; + // 计算角速度 + float wheel_base = (float)getParam("whl_bas"); + if (fabsf(wheel_base) < EPSILON) + { + return 0; // 避免除以0的情况 + } + diff_data.yaw_rate = speed_diff / wheel_base; + + return 0; +} + + +// 计算加速度 +float calculateAcceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float acceleration = (speed - previous_speed) / dt; + return acceleration; +} + +// 计算减速度 +float calculateDeceleration(float speed, float previous_speed, float dt) +{ + if (fabs(dt) < EPSILON) + { + return 0; // 避免除以0的情况 + } + float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度 + return deceleration; +} + +// 计算最大速度 +float calculateMaxSpeed() +{ + // 获取最大电机转速 (RPM) + float max_rpm = (float)getParam("max_rpm"); + // 获取轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + // 获取减速比 + float gear_ratio = (float)getParam("gRatio"); + if (fabsf(gear_ratio) < EPSILON) + { + return 0; // 避免除以0的情况 + } + // 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比 + float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio); + return max_speed; +} + +// 计算最大加速度 +float calculateMaxAcceleration(void) +{ + // 获取车辆参数 + float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩 + float vehicle_mass = (float)getParam("VehMass"); // 车辆质量 + float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径 + float gear_ratio = (float)getParam("gRatio"); // 减速比 + if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON ) + { + return 0; // 避免除以0的情况 + } + // 减速比计算扭矩 + float effective_torque = max_motor_torque * gear_ratio; + // 计算最大加速度 + float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass; + return max_acceleration; +} + +// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 +void calculateCurrentState(float dt) +{ + static float previous_speed = 0.0f; + // 更新当前速度和当前角速度 + calculateCurrentSpeedYawRate(); + // 更新加速度、减速度等，根据需要计算 + diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt); + diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt); + diff_data.max_speed = calculateMaxSpeed(); + previous_speed = diff_data.speed; +} + + +/** + * @brief 基于转速反比的双电机扭矩分配函数 + * @param rpm1 电机1当前转速（单位：rpm） + * @param rpm2 电机2当前转速（单位：rpm） + * @param total_torque 系统总需求扭矩（单位：Nm） + * @param torque1 [out] 电机1分配到的扭矩（单位：Nm） + * @param torque2 [out] 电机2分配到的扭矩（单位：Nm） + * @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡 + */ +void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque) +{ + + // 总扭矩为0时快速返回 + if (fabs(total_torque) < 0.001f) { + *torque1 = 0.0f; + *torque2 = 0.0f; + return; + } + +// // 保护条件：当两电机均静止时采用平均分配策略 +// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) { +// *torque1 = total_torque / 2.0f; +// *torque2 = total_torque / 2.0f; +// return; +// } + + // 计算权重因子（与转速成反比关系） + // 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理 + float weight1 = 1.0f / (fabs(rpm1) + 0.001f); + float weight2 = 1.0f / (fabs(rpm2) + 0.001f); + + // 归一化计算分配比例 + float total_weight = weight1 + weight2; + *torque1 = total_torque * (weight1 / total_weight); + *torque2 = total_torque * (weight2 / total_weight); + + // 独立限制单侧扭矩（修改核心逻辑） + if (fabs(*torque1) > max_torque) { + *torque1 = copysignf(max_torque, *torque1); + } + if (fabs(*torque2) > max_torque) { + *torque2 = copysignf(max_torque, *torque2); + } + + // 仅对非零扭矩应用下限限制 + if (fabs(*torque1) < min_torque) { + *torque1 = copysignf(min_torque, *torque1); + } + if ( fabs(*torque2) < min_torque) { + *torque2 = copysignf(min_torque, *torque2); + } + +} + + + +/** + * @brief 根据轮速差动态调整电机扭矩（带非负限制） + * @param speed_left 左轮速度（单位：rpm或自定义） + * @param speed_right 右轮速度（单位：rpm或自定义） + * @param torque_left 左轮扭矩指针（单位：Nm或自定义） + * @param torque_right 右轮扭矩指针（单位：Nm或自定义） + * @param threshold 触发调整的速差阈值（单位同轮速） + * @param k 扭矩调整系数（无量纲，建议0 threshold) { + // 计算需要减少的扭矩量（速差超出阈值部分×系数） + float torque_reduction = (speed_diff - threshold) * k; + + if (speed_left > speed_right) { + // 左轮过快时减少左扭矩，并限制最小值为0 + *torque_left = fmaxf(*torque_left - torque_reduction, 0.0f); + } else { + // 右轮过快时减少右扭矩，并限制最小值为0 + *torque_right = fmaxf(*torque_right - torque_reduction, 0.0f); + } + } +} +// 计算左右电机速度 +void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed) +{ + // 防止速度过低导致不必要的计算 + if (fabs(max_speed) < EPSILON) + { + motor_speed[0] = 0.0f; + motor_speed[1] = 0.0f; + motor_speed[2] = 0.0f; + motor_speed[3] = 0.0f; + return; + } + +#if THROTTLE_PID_MODE + float max_torque = (float)getParam("maxTorq"); + + linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); + yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + + + float left_speed_mps = linear_velocity_x + yaw_rate; + float right_speed_mps = linear_velocity_x - yaw_rate; + +//扭矩分配 + if(max_torque < left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = max_torque; + } + else if(-max_torque > left_speed_mps) + { + right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + left_speed_mps = -max_torque; + } + else if(max_torque < right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = max_torque; + } + else if(-max_torque > right_speed_mps) + { + left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差 + right_speed_mps = -max_torque; + } + else{} + +// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate); + + motor_speed[0] = left_speed_mps; + motor_speed[2] = left_speed_mps; + + motor_speed[1] = right_speed_mps; + motor_speed[3] = right_speed_mps; + + adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5); + adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5); + + +// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); + + + + + + + + + +// 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); + +// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]); + +// // 返回计算结果 +// *left_motor_speed = left_speed_mps; +// *right_motor_speed = right_speed_mps; +#else + + // 限制线速度和偏航率 + linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed); + float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f); + yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate); + + // 计算旋转速度 + float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate; + + // 计算车辆左右线速度 (m/s) + float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换 + float right_speed_mps = linear_velocity_x + rotational_velocity; + + // 计算轮子周长 + float wheel_circumference = (float)getParam("whl_dia") * M_PI; + + // 将车辆左右线速度转换为轮子转速 (RPM) + float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference; + float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference; + + // 获取减速比 + float gear_ratio = (float)getParam("gRatio"); + + // 将轮子转速转换为电机转速，考虑减速比 + float left_motor_rpm = left_wheel_rpm * gear_ratio; + float right_motor_rpm = right_wheel_rpm * gear_ratio; + + // 限制电机的最大和最小转速 + float max_motor_rpm = (float)getParam("max_rpm"); + left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm); + right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm); + // 当电机转速小于50转时，设置为0 + if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转 + { + left_motor_rpm = 0; + } + if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转 + { + right_motor_rpm = 0; + } + // 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下 + // left_motor_rpm = -left_motor_rpm; + // 返回计算结果 + *left_motor_speed = left_motor_rpm; + *right_motor_speed = right_motor_rpm; + +#endif +} + +// 映射遥控器速度，分为死区、低速区和高速区。 +float mapRemoteControlSpeed( + float input_speed, + float deadzone_limit, + float input_max, + float output_max, + float input_slow, + float output_slow +) +{ + float output_speed = 0.0f; + // 获取输入速度的绝对值 + float abs_input = fabsf(input_speed); +//diff_data.desired_speed, 0.1, 2, 10, 1, 5 + +// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + + if (abs_input < deadzone_limit + EPSILON) + { + output_speed = 0.0f;// 死区 + } + else if (abs_input < input_slow + EPSILON)// 低速区 + { + output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit);// + } + else if (abs_input <= input_max + EPSILON)// 高速区 + { + output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow);// 0.2 + （3 - 0.5）* （15-0.2） / （17 - 0.5） + } + else // 超出范围 + { + output_speed = output_max; + } + + // 根据原始输入速度的符号恢复方向 + if (input_speed < 0) + { + output_speed = -output_speed; + } + return output_speed; +} + +/** + * @brief 状态机处理函数（修改后版本） + */ +void handleVehicleState(DiffData *ctx) +{ + switch (ctx->state) + { + //------------------------------------------- + // 初始状态：根据期望速度方向跳转 + //------------------------------------------- + case STATE_INIT: + { + if (ctx->desired_speed < 0.0f) + { + ctx->state = STATE_BACKWARD; + } + else + { + ctx->state = STATE_FORWARD; + } + break; + } + + //------------------------------------------- + // 前进状态：处理反向指令（新增else分支） + //------------------------------------------- + case STATE_FORWARD: + { + if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f)) + { + ctx->state = STATE_BACKWARD; // 零速时允许切换方向 + } + else if ((ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f)) + { + ctx->desired_speed = 0.0f; // 非零速时清空期望速度 + ctx->state = STATE_FORWARD; // 显式保持当前状态 + } + else + { + ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态 + } + break; + } + + //------------------------------------------- + // 倒车状态：处理正向指令（新增else分支） + //------------------------------------------- + case STATE_BACKWARD: + { + if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f)) + { + ctx->state = STATE_FORWARD; // 零速时允许切换方向 + } + else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f)) + { + ctx->desired_speed = 0.0f; // 非零速时清空期望速度 + ctx->state = STATE_BACKWARD; // 显式保持当前状态 + } + else + { + ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态 + } + break; + } + } +} + + + + +// 差速处理函数 +static void diffProcess(void *signal_id) +{ + (void)signal_id; + static float previous_time1 = 0.0f; + + float time1 = (float)getCurrentTime(); + float dt = (time1 - previous_time1) / PERIOD_TICK; + previous_time1 = time1; + + // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 + calculateCurrentState(dt); + + // 当速度小于1时，设定为原地转向 20250321 修改为考虑负号 + if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) ) + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f; + } + else if( (diff_data.desired_speed < 0) && (diff_data.desired_speed >= -1.0f) ) + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * -1.0f; + } + else + { + diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed; + } + + handleVehicleState(&diff_data); //20250704 换挡函数 速度为0才能换挡 + + +// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); + // 使用 PID 控制器计算输出速度和曲率 + float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt); + float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt); + + // 计算最大加速度，用函数计算 + float max_acceleration = calculateMaxAcceleration(); + // 限制输出速度在当前速度和最大加速度计算出来的速度之间 + // 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 + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + output_speed = 0; + output_yaw_rate = 0; + } + +// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); + + +// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下 +// { +// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。 +// { +// output_yaw_rate = 500; +// } +// } + + + // 使用差速车辆动力学模型计算左右电机的期望速度 + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]); + + + + + + + +// if( (left_speed < 200) && (left_speed > -200) ) +// { +// left_speed = 0; +// } +// +// if( (right_speed < 200) && (right_speed > -200) ) +// { +// right_speed = 0; +// } + + + // 设置电机输出 + setMotorOutput(&diff_data.out_torq[0], + diff_data.max_Torq,// + (uint16_t)getParam("feedPwr"), + (uint16_t)getParam("dispPwr")); + // 发布左右电机期望转速,电源在工作状态才能发送 + if (power_data.current_state == POWER_WORKING) + { + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); + + } + + + un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10); + un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10); + + un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10); + un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10); + + un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed); + + publishMessage(&diff_data, 1); + +} + +/****************************************************************************** + Filter(); N个数中取两个 +******************************************************************************/ +int16_t Filter(int16_t *s,uint8_t Len) +{ + uint8_t i,j; + int16_t temp; +//降序排序 + for(i=0;i*(s+j)) + { + *(s+i)=*(s+i)^*(s+j); + *(s+j)=*(s+j)^*(s+i); + *(s+i)=*(s+i)^*(s+j); + } + } + temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移 + return(temp); +} + + + + + + +// 差速输入处理函数 +static void diffInput(void *signal_id) +{ + float motor_speed_temp = 0.0f; + + if (signal_id == &un_sw_sample) + { + diff_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch; + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b; + diff_data.mode = un_remote_control_input.bit_data.switch_c == 1 ? MODE_AUTO : MODE_MANUAL; + + if (diff_data.mode == MODE_MANUAL) + { + diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); + diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5); + + if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题 + { + diff_data.desired_curvature = diff_data.desired_curvature; + } + else + { + diff_data.desired_curvature = -diff_data.desired_curvature; + } + } + } + else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f; + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题 + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) + { + diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed)); + diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature)); + // 单位转换 + diff_data.desired_speed = diff_data.desired_speed * 0.01f; + diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相 + // 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出 + diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5); + diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + } + else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机） + { + diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量 + diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量 + + if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.left_front_motor_speed; + } + else + { + motor_speed_temp = diff_data.left_rear_motor_speed; + } + diff_data.left_motor_speed = motor_speed_temp; + } + else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机） + { + diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相 + diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000)); + + if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 + { + motor_speed_temp = diff_data.right_rear_motor_speed; + } + else + { + motor_speed_temp = diff_data.right_front_motor_speed; + } + + diff_data.right_motor_speed = motor_speed_temp; + } + + // 急停开关 + diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close()); + + // 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关 + if (diff_data.emergency_stop_state == 1) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + // 遥控器断线，而且是在手动模式，期望值清0 + if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) ) + { + diff_data.desired_speed = 0.0; + diff_data.desired_curvature = 0.0; + } + + if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制 + { + diff_data.max_Torq = 5;//20240403修改。刹车就是5N + } + else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩 + { + diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题 + } + else + { + diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩 + } + + diffProcess(&diff_data);//计算左右电机期望转速 +} + + + +// 预充完成处理函数 +void preChargeFinish(void *signal_id) +{ + (void)signal_id; + + float out_torq[4] = {0.0f,0.0f,0.0f,0.0f}; + + setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); + // 档位 +// un_motor_output1.bit_data.gear = 0; // 0表示空挡 +// un_motor_output2.bit_data.gear = 0; + publishMessage(&un_motor_output1, 1); + publishMessage(&un_motor_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); +} + + +void diffParametersInit(void *signal_id) +{ + (void)signal_id; // 标记变量为已使用，避免编译器警告 + + if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID + { + setPidParameters(&speed_pid, + getParam("Ospd_kp"), + getParam("Ospd_ki"), + getParam("Ospd_kd"), + getParam("Ospd_il"), + getParam("Ospd_ol") + ); + + setPidParameters(&yaw_rate_pid, + getParam("Ocrv_kp"), + getParam("Ocrv_ki"), + getParam("Ocrv_kd"), + getParam("Ocrv_il"), + getParam("Ocrv_ol") + ); + } + else//手动模式 + { + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + } + diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩 + + +// printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate); +// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); +// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate); + +// printf("left_motor_speed = %f\n",diff_data.left_motor_speed); +// printf("right_motor_speed = %f\n",diff_data.right_motor_speed); + +// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed); +// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]); + + float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed)); + float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature)); + // 单位转换 + deffspeed = deffspeed * 0.01f; + deffcurvature = deffcurvature * 0.0001f; + + printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature); + + timerStart(&diff_app_timer,1000,1);//1s调用一次 +} + + +// 差速初始化函数 +void diffAppInit(void) +{ + // 初始化 diff_data + memset(&diff_data, 0, sizeof(DiffData)); + + // 订阅相关信号 + subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关 + subscribe(&un_motor_input1, diffInput); + subscribe(&un_motor_input2, diffInput); + + + subscribe(&un_auto_computer_input, diffInput); + subscribe(&un_manual_computer_input, diffInput); + subscribe(&un_remote_control_input, diffInput); + subscribe(&power_data.pre_charge_finish, preChargeFinish); + + // 初始化速度 PID 控制器 + initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置速度 PID 控制器的参数 + setPidParameters(&speed_pid, + getParam("spd_kp"), + getParam("spd_ki"), + getParam("spd_kd"), + getParam("spd_il"), + getParam("spd_ol") + ); + + // 初始化曲率 PID 控制器 + initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置曲率 PID 控制器的参数 + setPidParameters(&yaw_rate_pid, + getParam("crv_kp"), + getParam("crv_ki"), + getParam("crv_kd"), + getParam("crv_il"), + getParam("crv_ol") + ); + + subscribe(&diff_app_timer, diffParametersInit); + timerStart(&diff_app_timer,1000,1);//1s调用一次 + + printf("diffControl: diffAppInit OK \n"); +} diff --git a/app/app_power.c b/app/app_power.c index 1ff0227..5a158c9 100644 --- a/app/app_power.c +++ b/app/app_power.c @@ -1,429 +1,429 @@ -#include "app_config.h" -#include "interface.h" -#include "app_frm_monitor.h" -#include "app_frm_signal.h" -#include "app_frm_timer.h" -#include "app_param_manage.h" - -#include "app_power.h" - - -// 定义按钮状态枚举 -typedef enum { - BUTTON_STATE_INITIAL, - BUTTON_STATE_SHORT_PRESS, - BUTTON_STATE_SHORT_PRESS_DETECTED, - BUTTON_STATE_WAIT_FOR_LONG_PRESS, - BUTTON_STATE_LONG_PRESS, - BUTTON_STATE_LONG_PRESS_WAIT -} ButtonState; - -// 定义按钮结构体 -typedef struct { - ButtonState state; - uint32_t press_start_time; - uint32_t release_start_time; - uint8_t is_power_on; - Timer timer; - uint8_t old_is_power_on; - Timer timer1; - -} PowerButton; - -// 全局变量 -PowerSystem power_data; -static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0}; - -// 电源按钮处理函数 -static void handlePowerButton(void) -{ - switch (power_button.state) - { - case BUTTON_STATE_INITIAL: - if (power_data.remote_power_switch == app_close()) - { - power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; - timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms - } - break; - - case BUTTON_STATE_SHORT_PRESS_DETECTED: - if (power_data.remote_power_switch == app_open()) - { - if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 - { - power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; - timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms - } - } - else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 - { - power_button.state = BUTTON_STATE_INITIAL; - } - break; - - case BUTTON_STATE_WAIT_FOR_LONG_PRESS: - if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 - { - power_button.state = BUTTON_STATE_LONG_PRESS; - timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms - } - else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 - { - power_button.state = BUTTON_STATE_INITIAL; - } - break; - - case BUTTON_STATE_LONG_PRESS: - - if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器 - { - power_button.is_power_on = !power_button.is_power_on; - printf("PowerButton: is_power_on = %d\n", power_button.is_power_on); - power_button.state = BUTTON_STATE_LONG_PRESS_WAIT; - } - else if(power_data.remote_power_switch == app_open()) - { - power_button.state = BUTTON_STATE_INITIAL; - printf("Long press for short duration"); - } - else - break; - - case BUTTON_STATE_LONG_PRESS_WAIT: - if (power_data.remote_power_switch == app_open())// 检测按键释放 - { - power_button.state = BUTTON_STATE_INITIAL; - printf("Release the button"); - } - default: - power_button.state = BUTTON_STATE_INITIAL; - break; - } -} - - -// 输出处理函数 -static void powerOutput(void *signal_id) -{ - (void)signal_id; - - // 根据当前状态,控制各个设备的电源 - switch (power_data.current_state) - { - case POWER_PRE_CHARGE: - publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - case POWER_NEUTRAL: - publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - - case POWER_STANDBY: - // 初始状态,只开启基本设备 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - case POWER_WORKING: - // 工作状态,除预充继电器外所有设备开启 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - case POWER_EMERGENCY: - // 急停状态,断开高压 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - case POWER_SLEEP: - // 休眠状态,关闭所有设备 - un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 - un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 - un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 - un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 - un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 - un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 - un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 - break; - - default: - break; - } - publishMessage(&power_data, 1); - publishMessage(&un_inf_can_kgf_output1, 1); - publishMessage(&un_inf_can_kgf_output2, 1); -} - -static void wakeupProcess(void *signal_id) -{ - (void)signal_id; - - un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm"); - un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm"); - - if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定 - { - un_gather_output.bit_data.sleep_duration = 5; - } - - if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定 - { - un_gather_output.bit_data.wakeup_interval = 5; - } - - un_gather_output.bit_data.vehicle_mode = power_data.current_state; - - publishMessage(&un_gather_output, 1); - timerStart(&power_data.timer1, 500, 1); //周期调用 -} - - -// 定时器处理函数 -static void powerTimerProcess(void *signal_id) -{ - (void)signal_id; - - // 调用电源按钮处理函数 - handlePowerButton(); - // 电源按钮状态有变化，保存到参数 - if (power_button.is_power_on != power_button.old_is_power_on) - { - setParam("pwr_btn", (float)power_button.is_power_on); - power_button.old_is_power_on = power_button.is_power_on; - } - - // 状态转换逻辑 - switch (power_data.current_state) - { - case POWER_PRE_CHARGE: - if (!power_data.timer_pre_charge.active) // 预充时间到 - { - power_data.current_state = POWER_NEUTRAL; // 工作 - power_data.pre_charge_finish = 1; // 预充完成 - printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n"); - } - break; - case POWER_NEUTRAL://20250316增加，发送空挡信号 - if (power_data.neutral_cnt >= 5) // 运行5次 - { - power_data.neutral_cnt = 0; - power_data.current_state = POWER_WORKING; // 工作 - power_data.pre_charge_finish = 1; // 预充完成 - printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n"); - } - else - { - power_data.neutral_cnt ++; - power_data.current_state = POWER_NEUTRAL; // 空挡 - power_data.pre_charge_finish = 1; // 预充完成 - } - break; - - case POWER_STANDBY: - if (power_data.high_voltage_switch == app_open()) // 高压开关断开 - { - power_data.current_state = POWER_SLEEP; // 休眠 - printf("Power: Transitioning from STANDBY to SLEEP state\n"); - } - else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 - { - power_data.current_state = POWER_EMERGENCY; // 急停 - printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); - } - break; - - case POWER_WORKING: - if (power_data.high_voltage_switch == app_open()) // 高压开关断开 - { - power_data.current_state = POWER_SLEEP; // 休眠 - printf("Power: Transitioning from STANDBY to SLEEP state\n"); - } - else if (power_data.emergency_stop == app_close()) // 急停开关闭合 - { - power_data.current_state = POWER_EMERGENCY; // 急停 - printf("Power: Transitioning from WORKING to EMERGENCY state\n"); - - printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态 - printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); - } - break; - case POWER_EMERGENCY: - if (power_data.high_voltage_switch == app_open()) // 高压开关断开 - { - power_data.current_state = POWER_SLEEP; // 休眠 - printf("Power: Transitioning from EMERGENCY to SLEEP state\n"); - } - else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 - { - power_data.current_state = POWER_STANDBY; // 待机 - printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); - } - else if (power_data.emergency_stop == app_open()) // 急停断开 - { - power_data.current_state = POWER_PRE_CHARGE; // 预充 - timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器 - printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); - } - break; - case POWER_SLEEP: - if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 - { - power_data.current_state = POWER_STANDBY; // 待机 - printf("Power: Transitioning from SLEEP to STANDBY state\n"); - } - break; - default: - power_data.current_state = POWER_STANDBY; // 待机 - break; - } - - powerOutput(NULL); // 输出 - - // 电源状态有变化，记录到参数 - if (power_data.old_state != power_data.current_state) - { - power_data.old_state = power_data.current_state; - setParam("pwr_sta", (float)power_data.current_state); - } - - timerStart(&power_data.timer, 100, 1); //周期调用 -} - - - - -// 处理所有输入信号的函数 -static void powerInput(void *signal_id) -{ - //不能直接赋值，用memcpy - PowerSystem old_data; - memcpy(&old_data, &power_data, sizeof(PowerSystem)); - - // 填充数据 - if (signal_id == &un_sw_sample) - { - power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关 - power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关 - } - else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 - { - power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 - power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 - } - // 急停开关 - power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); - - // 急停开关有变化，记录到参数 - if (power_data.old_emergency_stop != power_data.emergency_stop) - { - power_data.old_emergency_stop = power_data.emergency_stop; - setParam("stop_sw", (float)power_data.emergency_stop); - } - - // 高压开关状态有变化，记录到参数 - if (power_data.old_high_voltage_switch != power_data.high_voltage_switch) - { - power_data.old_high_voltage_switch = power_data.high_voltage_switch; - setParam("high_sw", (float)power_data.high_voltage_switch); - } -} - - -// APP模块的初始化 -void powerAppInit(void) -{ - // 初始化变量 - memset(&power_data, 0, sizeof(PowerSystem)); - power_data.current_state = POWER_STANDBY; - // 初始化时恢复电源状态 - power_data.current_state = (PowerState)getParam("pwr_sta"); - power_data.old_state = power_data.current_state; - // 恢复电源按钮状态 - power_button.is_power_on = (uint8_t)getParam("pwr_btn"); - power_button.old_is_power_on = power_button.is_power_on; - //恢复高压开关状态 - power_data.high_voltage_switch = (uint8_t)getParam("high_sw"); - power_data.old_high_voltage_switch = power_data.high_voltage_switch; - //恢复急停开关状态 - power_data.emergency_stop = (uint8_t)getParam("stop_sw"); - power_data.old_emergency_stop = power_data.emergency_stop; - - // 订阅输入信号 - subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 - subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关 - // 定时器 - timerInit(&power_data.timer); - subscribe(&power_data.timer, powerTimerProcess); - timerStart(&power_data.timer, 500, 1); // 周期调用 - //定时器唤醒 - timerInit(&power_data.timer1); - subscribe(&power_data.timer1, wakeupProcess); - timerStart(&power_data.timer1, 500, 1); // 周期调用 - //预充定时器 - timerInit(&power_data.timer_pre_charge); - subscribe(&power_data.timer_pre_charge, powerTimerProcess); - - printf("app_power: initial OK\n"); -} +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_power.h" + + +// 定义按钮状态枚举 +typedef enum { + BUTTON_STATE_INITIAL, + BUTTON_STATE_SHORT_PRESS, + BUTTON_STATE_SHORT_PRESS_DETECTED, + BUTTON_STATE_WAIT_FOR_LONG_PRESS, + BUTTON_STATE_LONG_PRESS, + BUTTON_STATE_LONG_PRESS_WAIT +} ButtonState; + +// 定义按钮结构体 +typedef struct { + ButtonState state; + uint32_t press_start_time; + uint32_t release_start_time; + uint8_t is_power_on; + Timer timer; + uint8_t old_is_power_on; + Timer timer1; + +} PowerButton; + +// 全局变量 +PowerSystem power_data; +static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0}; + +// 电源按钮处理函数 +static void handlePowerButton(void) +{ + switch (power_button.state) + { + case BUTTON_STATE_INITIAL: + if (power_data.remote_power_switch == app_close()) + { + power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED; + timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms + } + break; + + case BUTTON_STATE_SHORT_PRESS_DETECTED: + if (power_data.remote_power_switch == app_open()) + { + if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器 + { + power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS; + timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms + } + } + else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_WAIT_FOR_LONG_PRESS: + if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按 + { + power_button.state = BUTTON_STATE_LONG_PRESS; + timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms + } + else if (!power_button.timer.active) // 等待长按超时，重置为初始状态 + { + power_button.state = BUTTON_STATE_INITIAL; + } + break; + + case BUTTON_STATE_LONG_PRESS: + + if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器 + { + power_button.is_power_on = !power_button.is_power_on; + printf("PowerButton: is_power_on = %d\n", power_button.is_power_on); + power_button.state = BUTTON_STATE_LONG_PRESS_WAIT; + } + else if(power_data.remote_power_switch == app_open()) + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Long press for short duration"); + } + else + break; + + case BUTTON_STATE_LONG_PRESS_WAIT: + if (power_data.remote_power_switch == app_open())// 检测按键释放 + { + power_button.state = BUTTON_STATE_INITIAL; + printf("Release the button"); + } + default: + power_button.state = BUTTON_STATE_INITIAL; + break; + } +} + + +// 输出处理函数 +static void powerOutput(void *signal_id) +{ + (void)signal_id; + + // 根据当前状态,控制各个设备的电源 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_NEUTRAL: + publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + + case POWER_STANDBY: + // 初始状态,只开启基本设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_WORKING: + // 工作状态,除预充继电器外所有设备开启 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_EMERGENCY: + // 急停状态,断开高压 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOn(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + case POWER_SLEEP: + // 休眠状态,关闭所有设备 + un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器 + un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器 + un_inf_can_kgf_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器 + un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机 + un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器 + un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机 + un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3 + break; + + default: + break; + } + publishMessage(&power_data, 1); + publishMessage(&un_inf_can_kgf_output1, 1); + publishMessage(&un_inf_can_kgf_output2, 1); +} + +static void wakeupProcess(void *signal_id) +{ + (void)signal_id; + + un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm"); + un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm"); + + if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定 + { + un_gather_output.bit_data.sleep_duration = 5; + } + + if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定 + { + un_gather_output.bit_data.wakeup_interval = 5; + } + + un_gather_output.bit_data.vehicle_mode = power_data.current_state; + + publishMessage(&un_gather_output, 1); + timerStart(&power_data.timer1, 500, 1); //周期调用 +} + + +// 定时器处理函数 +static void powerTimerProcess(void *signal_id) +{ + (void)signal_id; + + // 调用电源按钮处理函数 + handlePowerButton(); + // 电源按钮状态有变化，保存到参数 + if (power_button.is_power_on != power_button.old_is_power_on) + { + setParam("pwr_btn", (float)power_button.is_power_on); + power_button.old_is_power_on = power_button.is_power_on; + } + + // 状态转换逻辑 + switch (power_data.current_state) + { + case POWER_PRE_CHARGE: + if (!power_data.timer_pre_charge.active) // 预充时间到 + { + power_data.current_state = POWER_NEUTRAL; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n"); + } + break; + case POWER_NEUTRAL://20250316增加，发送空挡信号 + if (power_data.neutral_cnt >= 5) // 运行5次 + { + power_data.neutral_cnt = 0; + power_data.current_state = POWER_WORKING; // 工作 + power_data.pre_charge_finish = 1; // 预充完成 + printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n"); + } + else + { + power_data.neutral_cnt ++; + power_data.current_state = POWER_NEUTRAL; // 空挡 + power_data.pre_charge_finish = 1; // 预充完成 + } + break; + + case POWER_STANDBY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from STANDBY to EMERGENCY state\n"); + } + break; + + case POWER_WORKING: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from STANDBY to SLEEP state\n"); + } + else if (power_data.emergency_stop == app_close()) // 急停开关闭合 + { + power_data.current_state = POWER_EMERGENCY; // 急停 + printf("Power: Transitioning from WORKING to EMERGENCY state\n"); + + printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态 + printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b); + } + break; + case POWER_EMERGENCY: + if (power_data.high_voltage_switch == app_open()) // 高压开关断开 + { + power_data.current_state = POWER_SLEEP; // 休眠 + printf("Power: Transitioning from EMERGENCY to SLEEP state\n"); + } + else if (power_button.is_power_on == app_open()) // 遥控器电源开关断开 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from EMERGENCY to STANDBY state\n"); + } + else if (power_data.emergency_stop == app_open()) // 急停断开 + { + power_data.current_state = POWER_PRE_CHARGE; // 预充 + timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器 + printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n"); + } + break; + case POWER_SLEEP: + if (power_data.high_voltage_switch == app_close()) // 高压开关闭合 + { + power_data.current_state = POWER_STANDBY; // 待机 + printf("Power: Transitioning from SLEEP to STANDBY state\n"); + } + break; + default: + power_data.current_state = POWER_STANDBY; // 待机 + break; + } + + powerOutput(NULL); // 输出 + + // 电源状态有变化，记录到参数 + if (power_data.old_state != power_data.current_state) + { + power_data.old_state = power_data.current_state; + setParam("pwr_sta", (float)power_data.current_state); + } + + timerStart(&power_data.timer, 100, 1); //周期调用 +} + + + + +// 处理所有输入信号的函数 +static void powerInput(void *signal_id) +{ + //不能直接赋值，用memcpy + PowerSystem old_data; + memcpy(&old_data, &power_data, sizeof(PowerSystem)); + + // 填充数据 + if (signal_id == &un_sw_sample) + { + power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关 + power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关 + } + else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据 + { + power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关 + power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关 + } + // 急停开关 + power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) ); + + // 急停开关有变化，记录到参数 + if (power_data.old_emergency_stop != power_data.emergency_stop) + { + power_data.old_emergency_stop = power_data.emergency_stop; + setParam("stop_sw", (float)power_data.emergency_stop); + } + + // 高压开关状态有变化，记录到参数 + if (power_data.old_high_voltage_switch != power_data.high_voltage_switch) + { + power_data.old_high_voltage_switch = power_data.high_voltage_switch; + setParam("high_sw", (float)power_data.high_voltage_switch); + } +} + + +// APP模块的初始化 +void powerAppInit(void) +{ + // 初始化变量 + memset(&power_data, 0, sizeof(PowerSystem)); + power_data.current_state = POWER_STANDBY; + // 初始化时恢复电源状态 + power_data.current_state = (PowerState)getParam("pwr_sta"); + power_data.old_state = power_data.current_state; + // 恢复电源按钮状态 + power_button.is_power_on = (uint8_t)getParam("pwr_btn"); + power_button.old_is_power_on = power_button.is_power_on; + //恢复高压开关状态 + power_data.high_voltage_switch = (uint8_t)getParam("high_sw"); + power_data.old_high_voltage_switch = power_data.high_voltage_switch; + //恢复急停开关状态 + power_data.emergency_stop = (uint8_t)getParam("stop_sw"); + power_data.old_emergency_stop = power_data.emergency_stop; + + // 订阅输入信号 + subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关 + subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关 + // 定时器 + timerInit(&power_data.timer); + subscribe(&power_data.timer, powerTimerProcess); + timerStart(&power_data.timer, 500, 1); // 周期调用 + //定时器唤醒 + timerInit(&power_data.timer1); + subscribe(&power_data.timer1, wakeupProcess); + timerStart(&power_data.timer1, 500, 1); // 周期调用 + //预充定时器 + timerInit(&power_data.timer_pre_charge); + subscribe(&power_data.timer_pre_charge, powerTimerProcess); + + printf("app_power: initial OK\n"); +} diff --git a/app/app_temp.c b/app/app_temp.c index b901351..971db69 100644 --- a/app/app_temp.c +++ b/app/app_temp.c @@ -1,285 +1,285 @@ -#include "app_config.h" -#include "interface.h" -#include "app_frm_monitor.h" -#include "app_frm_signal.h" -#include "app_frm_timer.h" -#include "app_param_manage.h" - -#include "app_temp.h" - - -// 声明 temp_data 变量 -TempSystem temp_data; - - -static void handleTemperatureAlarm(int16_t current_temp, float alarm_temp, - float critical_temp, float threshold_temp, - TempState *state) -{ - switch (*state) - { - case TEMP_NORMAL: - // 从正常状态进入警告状态的条件 - if (current_temp > (alarm_temp + threshold_temp)) - { - *state = TEMP_WARNING; - printf("Temperature Warning: Activated! Current temp: %d°C\n", current_temp); - } - // 从正常状态直接进入严重状态的条件 - else if (current_temp > (critical_temp + threshold_temp)) - { - *state = TEMP_CRITICAL; - printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); - } - else - { - *state = TEMP_NORMAL; - } - break; - - case TEMP_WARNING: - // 从警告状态返回正常状态的条件 - if (current_temp < (alarm_temp - threshold_temp)) - { - *state = TEMP_NORMAL; - printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); - } - // 从警告状态进入严重状态的条件 - else if (current_temp > (critical_temp + threshold_temp)) - { - *state = TEMP_CRITICAL; - printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); - } - else - { - *state = TEMP_WARNING; - } - break; - - case TEMP_CRITICAL: - // 从严重状态返回警告状态的条件 - if (current_temp < (critical_temp - threshold_temp)) - { - *state = TEMP_WARNING; - printf("Temperature Critical: Deactivated! Current temp: %d°C\n", current_temp); - } - // 从严重状态直接返回正常状态的条件 - else if (current_temp < (alarm_temp - threshold_temp)) - { - *state = TEMP_NORMAL; - printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); - } - else - { - *state = TEMP_CRITICAL; - } - break; - - default: - *state = TEMP_NORMAL; - break; - } -} - - -// 温度输出处理函数 -static void tempOutput(void *signal_id) -{ - (void)signal_id; - - // 电机1风扇 左前 - switch (temp_data.state[0]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_01 = 0; - break; - } - - // 电机2风扇 右前 - switch (temp_data.state[1]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_02 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_02 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_02 = 0; - break; - } - - - // 电机3风扇 左后 - switch (temp_data.state[2]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output1.bit_data.KGF07 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_07 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output1.bit_data.KGF07 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_07 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output1.bit_data.KGF07 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_07 = 0; - break; - } - - // 电机4风扇 右后 - switch (temp_data.state[3]) - { - case TEMP_NORMAL: - un_inf_can_kgf_output1.bit_data.KGF08 = setFanOff();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_08 = 0; - break; - case TEMP_WARNING: - un_inf_can_kgf_output1.bit_data.KGF08 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_08 = 5; - break; - case TEMP_CRITICAL: - un_inf_can_kgf_output1.bit_data.KGF08 = setFanOn();//电机控制器风扇 - un_inf_can_kgf_output1.bit_data.pwm_08 = 0; - break; - } - -// // 电机3风扇 -// switch (temp_data.state[2]) -// { -// case TEMP_NORMAL: -// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 -// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; -// break; -// case TEMP_WARNING: -// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 -// un_inf_can_kgf_output1.bit_data.pwm_01 = 5; -// break; -// case TEMP_CRITICAL: -// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 -// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; -// break; -// } - - publishMessage(&un_inf_can_kgf_output1, 1); -} - -// 温度状态处理函数 -static void tempProcess(void *signal_id) -{ - (void)signal_id; - int16_t max_temp[4] = {0,0}; - - max_temp[0] = temp_data.current_temp[0]; - max_temp[1] = temp_data.current_temp[1]; - -// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); - handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]); - - handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]); - - handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]); - - handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[3]); - - - -// if (max_temp[0] >= 60) // 假设60度为危险温度 -// { -// temp_data.state[0] = TEMP_CRITICAL; -// } -// else if (max_temp[0] >= 40) // 假设40度为警告温度 -// { -// temp_data.state[0] = TEMP_WARNING; -// } -// else -// { -// temp_data.state[0] = TEMP_NORMAL; -// } -// -// -// max_temp[1] = temp_data.current_temp[1]; -// if (max_temp[1] >= 60) // 假设60度为危险温度 -// { -// temp_data.state[1] = TEMP_CRITICAL; -// } -// else if (max_temp[1] >= 40) // 假设40度为警告温度 -// { -// temp_data.state[1] = TEMP_WARNING; -// } -// else -// { -// temp_data.state[1] = TEMP_NORMAL; -// } -// -//// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); -//// printf("motor1 state: %d, motor2 state: %d\n", temp_data.state[0], temp_data.state[1]); - - tempOutput(NULL); - - timerStart(&temp_data.timer, 1000, 1); //1s -} - -// 处理输入信号的函数 -static void tempInput(void *signal_id) -{ - (void)signal_id; - // 填充数据 - if (signal_id == &un_motor_temp1) - { - temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量 - } - else if(signal_id == &un_motor_temp2) - { - temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40); - } - else if(signal_id == &un_motor_temp3) - { - temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40); - } - else if(signal_id == &un_motor_temp4) - { - temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40); - } - else{} -} - - -// APP模块的初始化 -void tempAppInit(void) -{ - // 初始化 - timerInit(&temp_data.timer); - - memset(&temp_data, 0, sizeof(TempSystem)); - temp_data.state[0] = TEMP_NORMAL; - temp_data.state[1] = TEMP_NORMAL; - temp_data.mode = TEMP_MODE_AUTO; - temp_data.target_temp = 25; // 默认目标温度25度 - - // 订阅输入信号,处理温度逻辑 - subscribe(&un_motor_temp1, tempInput); - subscribe(&un_motor_temp2, tempInput); - subscribe(&un_motor_temp3, tempInput); - subscribe(&un_motor_temp4, tempInput); - - // 启动定时器,每秒检查一次温度 - subscribe(&temp_data.timer, tempProcess); - timerStart(&temp_data.timer, 1000, 1); //1s - - printf("app_temp: initial OK \n"); - - -} +#include "app_config.h" +#include "interface.h" +#include "app_frm_monitor.h" +#include "app_frm_signal.h" +#include "app_frm_timer.h" +#include "app_param_manage.h" + +#include "app_temp.h" + + +// 声明 temp_data 变量 +TempSystem temp_data; + + +static void handleTemperatureAlarm(int16_t current_temp, float alarm_temp, + float critical_temp, float threshold_temp, + TempState *state) +{ + switch (*state) + { + case TEMP_NORMAL: + // 从正常状态进入警告状态的条件 + if (current_temp > (alarm_temp + threshold_temp)) + { + *state = TEMP_WARNING; + printf("Temperature Warning: Activated! Current temp: %d°C\n", current_temp); + } + // 从正常状态直接进入严重状态的条件 + else if (current_temp > (critical_temp + threshold_temp)) + { + *state = TEMP_CRITICAL; + printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_NORMAL; + } + break; + + case TEMP_WARNING: + // 从警告状态返回正常状态的条件 + if (current_temp < (alarm_temp - threshold_temp)) + { + *state = TEMP_NORMAL; + printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); + } + // 从警告状态进入严重状态的条件 + else if (current_temp > (critical_temp + threshold_temp)) + { + *state = TEMP_CRITICAL; + printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_WARNING; + } + break; + + case TEMP_CRITICAL: + // 从严重状态返回警告状态的条件 + if (current_temp < (critical_temp - threshold_temp)) + { + *state = TEMP_WARNING; + printf("Temperature Critical: Deactivated! Current temp: %d°C\n", current_temp); + } + // 从严重状态直接返回正常状态的条件 + else if (current_temp < (alarm_temp - threshold_temp)) + { + *state = TEMP_NORMAL; + printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp); + } + else + { + *state = TEMP_CRITICAL; + } + break; + + default: + *state = TEMP_NORMAL; + break; + } +} + + +// 温度输出处理函数 +static void tempOutput(void *signal_id) +{ + (void)signal_id; + + // 电机1风扇 左前 + switch (temp_data.state[0]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_01 = 0; + break; + } + + // 电机2风扇 右前 + switch (temp_data.state[1]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output1.bit_data.pwm_02 = 0; + break; + } + + + // 电机3风扇 左后 + switch (temp_data.state[2]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output2.bit_data.KGF07 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_07 = 0; + break; + } + + // 电机4风扇 右后 + switch (temp_data.state[3]) + { + case TEMP_NORMAL: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOff();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 0; + break; + case TEMP_WARNING: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 5; + break; + case TEMP_CRITICAL: + un_inf_can_kgf_output2.bit_data.KGF08 = setFanOn();//电机控制器风扇 + un_inf_can_kgf_output2.bit_data.pwm_08 = 0; + break; + } + +// // 电机3风扇 +// switch (temp_data.state[2]) +// { +// case TEMP_NORMAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; +// break; +// case TEMP_WARNING: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 5; +// break; +// case TEMP_CRITICAL: +// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇 +// un_inf_can_kgf_output1.bit_data.pwm_01 = 0; +// break; +// } + + publishMessage(&un_inf_can_kgf_output1, 1); +} + +// 温度状态处理函数 +static void tempProcess(void *signal_id) +{ + (void)signal_id; + int16_t max_temp[4] = {0,0}; + + max_temp[0] = temp_data.current_temp[0]; + max_temp[1] = temp_data.current_temp[1]; + +// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); + handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]); + + handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]); + + handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]); + + handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[3]); + + + +// if (max_temp[0] >= 60) // 假设60度为危险温度 +// { +// temp_data.state[0] = TEMP_CRITICAL; +// } +// else if (max_temp[0] >= 40) // 假设40度为警告温度 +// { +// temp_data.state[0] = TEMP_WARNING; +// } +// else +// { +// temp_data.state[0] = TEMP_NORMAL; +// } +// +// +// max_temp[1] = temp_data.current_temp[1]; +// if (max_temp[1] >= 60) // 假设60度为危险温度 +// { +// temp_data.state[1] = TEMP_CRITICAL; +// } +// else if (max_temp[1] >= 40) // 假设40度为警告温度 +// { +// temp_data.state[1] = TEMP_WARNING; +// } +// else +// { +// temp_data.state[1] = TEMP_NORMAL; +// } +// +//// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]); +//// printf("motor1 state: %d, motor2 state: %d\n", temp_data.state[0], temp_data.state[1]); + + tempOutput(NULL); + + timerStart(&temp_data.timer, 1000, 1); //1s +} + +// 处理输入信号的函数 +static void tempInput(void *signal_id) +{ + (void)signal_id; + // 填充数据 + if (signal_id == &un_motor_temp1) + { + temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量 + } + else if(signal_id == &un_motor_temp2) + { + temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40); + } + else if(signal_id == &un_motor_temp3) + { + temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40); + } + else if(signal_id == &un_motor_temp4) + { + temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40); + } + else{} +} + + +// APP模块的初始化 +void tempAppInit(void) +{ + // 初始化 + timerInit(&temp_data.timer); + + memset(&temp_data, 0, sizeof(TempSystem)); + temp_data.state[0] = TEMP_NORMAL; + temp_data.state[1] = TEMP_NORMAL; + temp_data.mode = TEMP_MODE_AUTO; + temp_data.target_temp = 25; // 默认目标温度25度 + + // 订阅输入信号,处理温度逻辑 + subscribe(&un_motor_temp1, tempInput); + subscribe(&un_motor_temp2, tempInput); + subscribe(&un_motor_temp3, tempInput); + subscribe(&un_motor_temp4, tempInput); + + // 启动定时器,每秒检查一次温度 + subscribe(&temp_data.timer, tempProcess); + timerStart(&temp_data.timer, 1000, 1); //1s + + printf("app_temp: initial OK \n"); + + +} diff --git a/main.c b/main.c index 931b915..3eb9118 100644 --- a/main.c +++ b/main.c @@ -1,142 +1,141 @@ -#include -#include "board.h" -#include "debug.h" -#include "regs_base.h" -#include "irq_num.h" - -#include "clock_cfg.h" -#include "reset_cfg.h" -#include "pinmux_cfg.h" -#include "eth_cfg.h" -#include "lwip/err.h" -#include "lwip/inet.h" -#include "lwip/tcp.h" -#include "lwip/timeouts.h" -#include - -#include "lwip/udp.h" -#include "flexcan_cfg.h" - -#include "interface_config.h" -#include "irq.h" - -//app包含 -#include "app/app_config.h" -#include "app/app_param_manage.h" -#include "app/app_power.h" -#include "app/app_brake.h" -#include "app/app_differential_drive.h" -#include -#include -#include -#include "app/app_request.h" -#include "app/app_ultrasonic.h" - -void testAppInit(void); - -#define IP_ADDR0 10 -#define IP_ADDR1 18 -#define IP_ADDR2 252 -#define IP_ADDR3 34 - - -extern int btm_init(void); -extern struct tcp_pcb *netdemo_connect_server(char *ip, int port); -extern bool netdemo_tcp_connected(void); -extern bool netdemo_tcp_send_done(void); -extern void netdemo_clear_tcp_send_done(void); - - - - - -uint8_t mainCnt = 0; -uint8_t mainCntarr[8] = {0x56,0x88,0x98,0x79,0x23}; - - -uint8_t test_app[26] = {0}; - -int main(void) -{ - initFramework(); - - uint8_t BOOT_Arr[2] = {0x01,0x02};//上电发送app帧给上位机用 - - // 释放MCU各模块reset信号 - board_reset_init(); - - // 配置时钟 - int ret = sdrv_ckgen_init(&g_clock_config); - ASSERT(ret == 0); - - // VIC初始化 - irq_initialize(VIC1_BASE, IRQ_MAX_INTR_NUM); - - // 配置pinmux - sdrv_pinctrl_init(NUM_OF_CONFIGURED_PINS, g_pin_init_config); - - // Timer初始化，用于提供LWIP sys_now接口获取当前时间 - btm_init(); - - // 使能打印 - board_debug_console_init(); - - printf("debug_console_init initial OK %d\n",getCurrentTime()); - //初始化所有can - initialization_All_Flexcan(); - - printf("initialization_All_Flexcan initial OK %d\n",getCurrentTime()); - // ETH初始化 - board_eth_init(); - - initSpi();//初始化SPI - - //以太网端口初始化 - UDP_Echo_Init(UDPCB_1, udp_Callback_1, COMMUNICATION_PORT); - UDP_Echo_Init(UDPCB_2, udp_Callback_2, UPPER_PORT); - UDP_Echo_Init(UDPCB_3, udp_Callback_3, PARAM_PORT); - UDP_Echo_Init(UDPCB_4, udp_Callback_4, DOWNLOAR_PORT); - UDP_Echo_Init(UDPCB_5, udp_Callback_5, WDT_PORT); - - printf("board_eth_init initial OK %d\n",getCurrentTime()); - //读取重启标志 - g_systemDataRecord.canBootloaderUpgrade = rdbyte_24c02(0x00); - printf("rdbyte_24c02 OK %d\n",getCurrentTime()); - - ethernet_parameter.download_ip[0] = rdbyte_24c02(BOOT_DES_IP);//读取下载的IP - ethernet_parameter.download_ip[1] = rdbyte_24c02(BOOT_DES_IP+1);//读取下载的IP - ethernet_parameter.download_ip[2] = rdbyte_24c02(BOOT_DES_IP+2);//读取下载的IP - ethernet_parameter.download_ip[3] = rdbyte_24c02(BOOT_DES_IP+3);//读取下载的IP - - //发送重启后的第一帧给上位机 - CAN_Send_Msg(&can_handle_6,OTA_CANTxID, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, BOOT_Arr,2, TX_MB_INDEX);//app 帧 - - printf("CAN_Send_Msg can_handle_6 OK %d\n",getCurrentTime()); - - - //打印版本号 - printf("version: V1.72 \n"); - // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。 - - testAppInit(); - paramAppInit(); - diffAppInit(); - brakeAppInit(); - powerAppInit(); //电源管理 - tempAppInit(); //温度 - lightAppInit(); //灯光 - ethernetInterfaceInit(); //以太网先初始化 - requestAppInit(); - canInterfaceInit(); - bootInterfaceInit(); -// ultrasonicAppInit(); - - printf("All init OK ------ %d\n",getCurrentTime()); - - sdrv_gpio_set_pin_output_level(GPIO_B9, 1); //测量时间 - for (;;) - { - // 处理信号 - processMessages(); - } -} +#include +#include "board.h" +#include "debug.h" +#include "regs_base.h" +#include "irq_num.h" + +#include "clock_cfg.h" +#include "reset_cfg.h" +#include "pinmux_cfg.h" +#include "eth_cfg.h" +#include "lwip/err.h" +#include "lwip/inet.h" +#include "lwip/tcp.h" +#include "lwip/timeouts.h" +#include + +#include "lwip/udp.h" +#include "flexcan_cfg.h" + +#include "interface_config.h" +#include "irq.h" + +//app包含 +#include "app/app_config.h" +#include "app/app_param_manage.h" +#include "app/app_power.h" +#include "app/app_brake.h" +#include "app/app_differential_drive.h" +#include +#include +#include +#include "app/app_request.h" +#include "app/app_ultrasonic.h" + +void testAppInit(void); + +#define IP_ADDR0 10 +#define IP_ADDR1 18 +#define IP_ADDR2 252 +#define IP_ADDR3 34 + + +extern int btm_init(void); +extern struct tcp_pcb *netdemo_connect_server(char *ip, int port); +extern bool netdemo_tcp_connected(void); +extern bool netdemo_tcp_send_done(void); +extern void netdemo_clear_tcp_send_done(void); + + + + + +uint8_t mainCnt = 0; +uint8_t mainCntarr[8] = {0x56,0x88,0x98,0x79,0x23}; + + +uint8_t test_app[26] = {0}; + +int main(void) +{ + initFramework(); + + uint8_t BOOT_Arr[2] = {0x01,0x02};//上电发送app帧给上位机用 + + // 释放MCU各模块reset信号 + board_reset_init(); + + // 配置时钟 + int ret = sdrv_ckgen_init(&g_clock_config); + ASSERT(ret == 0); + + // VIC初始化 + irq_initialize(VIC1_BASE, IRQ_MAX_INTR_NUM); + + // 配置pinmux + sdrv_pinctrl_init(NUM_OF_CONFIGURED_PINS, g_pin_init_config); + + // Timer初始化，用于提供LWIP sys_now接口获取当前时间 + btm_init(); + + // 使能打印 + board_debug_console_init(); + + printf("debug_console_init initial OK %d\n",getCurrentTime()); + //初始化所有can + initialization_All_Flexcan(); + + printf("initialization_All_Flexcan initial OK %d\n",getCurrentTime()); + // ETH初始化 + board_eth_init(); + + initSpi();//初始化SPI + + //以太网端口初始化 + UDP_Echo_Init(UDPCB_1, udp_Callback_1, COMMUNICATION_PORT); + UDP_Echo_Init(UDPCB_2, udp_Callback_2, UPPER_PORT); + UDP_Echo_Init(UDPCB_3, udp_Callback_3, PARAM_PORT); + UDP_Echo_Init(UDPCB_4, udp_Callback_4, DOWNLOAR_PORT); + UDP_Echo_Init(UDPCB_5, udp_Callback_5, WDT_PORT); + + printf("board_eth_init initial OK %d\n",getCurrentTime()); + //读取重启标志 + g_systemDataRecord.canBootloaderUpgrade = rdbyte_24c02(0x00); + printf("rdbyte_24c02 OK %d\n",getCurrentTime()); + + ethernet_parameter.download_ip[0] = rdbyte_24c02(BOOT_DES_IP);//读取下载的IP + ethernet_parameter.download_ip[1] = rdbyte_24c02(BOOT_DES_IP+1);//读取下载的IP + ethernet_parameter.download_ip[2] = rdbyte_24c02(BOOT_DES_IP+2);//读取下载的IP + ethernet_parameter.download_ip[3] = rdbyte_24c02(BOOT_DES_IP+3);//读取下载的IP + + //发送重启后的第一帧给上位机 + CAN_Send_Msg(&can_handle_6,OTA_CANTxID, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, BOOT_Arr,2, TX_MB_INDEX);//app 帧 + + printf("CAN_Send_Msg can_handle_6 OK %d\n",getCurrentTime()); + + + //打印版本号 + printf("version: V1.72 \n"); + // 初始化框架 放在最前面，解决电机can发送信号累积不处理的问题。 + + testAppInit(); + paramAppInit(); + diffAppInit(); + brakeAppInit(); + powerAppInit(); //电源管理 + tempAppInit(); //温度 + lightAppInit(); //灯光 + ethernetInterfaceInit(); //以太网先初始化 + requestAppInit(); + canInterfaceInit(); + bootInterfaceInit(); +// ultrasonicAppInit(); + + printf("All init OK ------ %d\n",getCurrentTime()); + + for (;;) + { + // 处理信号 + processMessages(); + } +}