diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index da0c296..a47bc6c 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -25,117 +25,190 @@ PID_t yaw_rate_pid; PID_t Acc_front_speed_pid; PID_t Dec_front_speed_pid; +/** + * @brief 根据挡位和输入转矩计算输出转矩值 + * @param gear 挡位状态（STATE_FORWARD/STATE_BACKWARD/其他） + * @param input_torque 输入转矩值 + * @return 处理后的转矩值（已包含偏移量和系数） + */ +float calculateTorqueOutput(uint8_t gear, float input_torque) +{ + const float OFFSET = 300.0f; // 偏移量常量 + const float SCALE_FACTOR = 100.0f; // 缩放系数 + const float DEFAULT_VALUE = 30000.0f; // 默认输出值 + float output_torque; + + if (gear == STATE_FORWARD) + { + output_torque = (input_torque + OFFSET) * SCALE_FACTOR; + } + else if (gear == STATE_BACKWARD) + { + output_torque = (-input_torque + OFFSET) * SCALE_FACTOR; + } + else + { + output_torque = DEFAULT_VALUE; + } + + return output_torque; +} + + +/** + * @brief 车辆状态控制状态机 + * @note 根据车速和扭矩方向切换前进/后退状态，带扭矩回滞保护 + * @param ctx 状态机上下文，包含当前状态(STATE_INIT/FORWARD/BACKWARD) + * @param speed 当前车速（单位：km/h），0表示静止状态 + * @param torque 当前扭矩（单位：Nm），正数表示前进方向，负数表示后退方向 + */ +void handleVehicleState(MotorState *ctx, float speed, float torque) +{ + switch (*ctx) + { + // 初始状态：根据扭矩方向初始化 + case STATE_INIT: + { + if (torque >= 0.0f) + { + *ctx = STATE_FORWARD; // 正扭矩进前进档 + } + else + { + *ctx = STATE_BACKWARD; // 负扭矩进倒档 + } + break; + } + + // 前进状态：零速且反向扭矩超阈值切倒档 + case STATE_FORWARD: + { + if ( (speed == 0.0f) && (torque <= -TORQUE_HYSTERESIS_THRESHOLD) ) + { + *ctx = STATE_BACKWARD; // 满足条件切换 + } + else + { + *ctx = STATE_FORWARD; // 否则保持 + } + break; + } + + // 倒车状态：零速且正向扭矩超阈值切前进 + case STATE_BACKWARD: + { + if ( (speed == 0.0f) && (torque >= TORQUE_HYSTERESIS_THRESHOLD) ) + { + *ctx = STATE_FORWARD; // 满足条件切换 + } + else + { + *ctx = STATE_BACKWARD; // 否则保持 + } + break; + } + + default:; // 异常处理 + } +} // 设置电机输出 -void setMotorOutput(float *out_speed, uint16_t max_torque, uint16_t feed_power, uint16_t discharge_power) -{ - float abs_speed[4] = {0.0,0.0,0.0,0.0}; -//-------------------------------------------------------------- - if(out_speed[2] > 0) - { - un_motor_output1.bit_data.gear = 1; - } - else if(out_speed[2] < 0) - { - un_motor_output1.bit_data.gear = 2; - } - else{} - - if(out_speed[0] > 0) - { - un_motor_output3.bit_data.gear = 1; - } - else if(out_speed[0] < 0) - { - un_motor_output3.bit_data.gear = 2; - } - else{} - - if(out_speed[1] > 0) - { - un_motor_output2.bit_data.gear = 1; - } - else if(out_speed[1] < 0) - { - un_motor_output2.bit_data.gear = 2; - } - else{} - - - if(out_speed[3] > 0) - { - un_motor_output4.bit_data.gear = 1; - } - else if(out_speed[3] < 0) - { - un_motor_output4.bit_data.gear = 2; - } - else{} - - -// if(diff_data.out_motor_dir != 0)//如果为0那就是空挡，那就保留之前的挡位否则以输入的速度为挡位 -// { -// un_motor_output1.bit_data.gear = diff_data.out_motor_dir; -// un_motor_output2.bit_data.gear = diff_data.out_motor_dir; -// un_motor_output3.bit_data.gear = diff_data.out_motor_dir; -// un_motor_output4.bit_data.gear = diff_data.out_motor_dir; -// } -// - -// un_motor_output1.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 -// un_motor_output3.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; // 1 表示前进，2 表示后退 - -// un_motor_output2.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; -// un_motor_output4.bit_data.gear = (diff_data.out_motor_dir <= 0) ? 1 : 2; - - - for(uint8_t i = 0; i<4; i++) - { -// if(out_speed[i] < 0) //给负扭矩就发0，刹车 -// { -// out_speed[i] = 0; -// } - abs_speed[i] = fabs(out_speed[i]);//要扩大10倍 扭矩比例是10 - } +void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) +{ - un_motor_output1.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000//限定值为256 - un_motor_output1.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[2]) ); // 20240921 增加偏移量 30000 - un_motor_output3.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000//限定值为256 - un_motor_output3.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[0]) ); // 20240921 增加偏移量 30000 + 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_output2.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 - un_motor_output2.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[1]) ); // 20240921 增加偏移量 30000 - un_motor_output4.bit_data.control_data1 = ( (uint16_t)roundf(abs_speed[3]) ); // 20240921 增加偏移量 30000 - un_motor_output4.bit_data.control_data2 = ( (uint16_t)roundf(abs_speed[3]) ); // 20240921 增加偏移量 30000 + // 档位 + abs_left_front_speed = calculateTorqueOutput(diff_data.motor_state[0], out_torq[0]); //根据挡位增加转矩方向 + abs_right_front_speed = calculateTorqueOutput(diff_data.motor_state[1], out_torq[1]); + abs_left_rear_speed = calculateTorqueOutput(diff_data.motor_state[2], out_torq[2]); + abs_right_rear_speed = calculateTorqueOutput(diff_data.motor_state[3], out_torq[3]); + + un_motor_output1.bit_data.gear = diff_data.motor_state[0]; +// un_motor_output3.bit_data.gear = diff_data.motor_state[2]; +// un_motor_output2.bit_data.gear = diff_data.motor_state[1]; +// un_motor_output4.bit_data.gear = diff_data.motor_state[3]; + + if(STATE_FORWARD == diff_data.motor_state[1])//把后两台电机反相 + { + un_motor_output2.bit_data.gear = STATE_BACKWARD; + } + else if(STATE_BACKWARD == diff_data.motor_state[1]) + { + un_motor_output2.bit_data.gear = STATE_FORWARD; + } + else + { + un_motor_output2.bit_data.gear = STATE_INIT; + } + if(STATE_FORWARD == diff_data.motor_state[2])//把后两台电机反相 + { + un_motor_output3.bit_data.gear = STATE_BACKWARD; + } + else if(STATE_BACKWARD == diff_data.motor_state[2]) + { + un_motor_output3.bit_data.gear = STATE_FORWARD; + } + else + { + un_motor_output3.bit_data.gear = STATE_INIT; + } - // 设置模式为油门模式 - un_motor_output1.bit_data.mode = MOTOR_MODE; //转矩模式 + if(STATE_FORWARD == diff_data.motor_state[3]) + { + un_motor_output4.bit_data.gear = STATE_BACKWARD; + } + else if(STATE_BACKWARD == diff_data.motor_state[3]) + { + un_motor_output4.bit_data.gear = STATE_FORWARD; + } + else + { + un_motor_output4.bit_data.gear = STATE_INIT; + } + + + // 设置左右电机期望转速 +// 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.set_rotation_speed = 30000; + un_motor_output2.bit_data.set_rotation_speed = 30000; + un_motor_output3.bit_data.set_rotation_speed = 30000; + un_motor_output4.bit_data.set_rotation_speed = 30000; -// 电机方向 - un_motor_output1.bit_data.motor_direction = 1;//电机2和3是反相的 - un_motor_output2.bit_data.motor_direction = 1; -// un_motor_output1.bit_data.motor_direction = 1;//电机2和3是反相的 -// un_motor_output2.bit_data.motor_direction = 0; - un_motor_output3.bit_data.motor_direction = 0; - un_motor_output4.bit_data.motor_direction = 0; + // 设置馈电功率 + 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; - // can挡位一直有效 - un_motor_output1.bit_data.can_gear = 1; - un_motor_output2.bit_data.can_gear = 1; - un_motor_output3.bit_data.can_gear = 1; - un_motor_output4.bit_data.can_gear = 1; - - un_motor_output1.bit_data.can_break = 1; - un_motor_output2.bit_data.can_break = 1; - un_motor_output3.bit_data.can_break = 1; - un_motor_output4.bit_data.can_break = 1; + } // 限制值在最小值和最大值之间 @@ -258,6 +331,62 @@ void calculateCurrentState(float dt) } +/** + * @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或自定义） @@ -303,14 +432,21 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max } #if THROTTLE_PID_MODE - float max_torque = (float)getParam("maxTorq"); + float left_speed_mps = 0.0f; + float right_speed_mps = 0.0f; - linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); - yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了 +// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); +// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); - float left_speed_mps = linear_velocity_x + yaw_rate; - float right_speed_mps = linear_velocity_x - yaw_rate; + if( diff_data.min_Torq > fabs(linear_velocity_x) )//20250728 增加死区 解决手柄回中，不停车问题 + { + linear_velocity_x = 0; + } + left_speed_mps = linear_velocity_x + yaw_rate; + right_speed_mps = linear_velocity_x - yaw_rate; + //扭矩分配 if(max_torque < left_speed_mps) { @@ -321,7 +457,7 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max { 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);//多减去超出限值得部分，保证转矩差 @@ -334,15 +470,23 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max } else{} - // 返回计算结果 - motor_speed[0] = left_speed_mps; +// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate); +// 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); + + motor_speed[0] = left_speed_mps;//加速状态，没有负扭矩，要么前进加速要么后退加速 motor_speed[2] = left_speed_mps; motor_speed[1] = right_speed_mps; - motor_speed[3] = right_speed_mps; - - - + motor_speed[3] = right_speed_mps; + + handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 + handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]); + handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); + handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); + + // adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],50, 0.2); // adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],50, 0.2); @@ -434,160 +578,6 @@ float mapRemoteControlSpeed( } return output_speed; } - - -/** - * @brief 状态机处理函数（修改后版本） - */ -void handleVehicleState(DiffData *ctx,float *torque) -{ - static uint8_t handle_cnt = 0; - 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->state = DEC_FORWARD; // 显式保持当前状态 - *torque = 1;//减速的话先发1，然后再发0 - handle_cnt = 0; - } - else - { - ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态 - } - break; - } - //------------------------------------------- - // 前进减速状态： - //------------------------------------------- - case DEC_FORWARD: - { - if (handle_cnt >= 10) - { - *torque = 0;//减速的话先发1，然后再发0 - ctx->state = DEC_FORWARD2; // 零速时允许切换方向 - handle_cnt = 0; - } - else - { - handle_cnt ++; - ctx->state = DEC_FORWARD; // 新增：其他情况保持前进状态 - } - break; - } - //------------------------------------------- - // 前进溜车状态： - //------------------------------------------- - case DEC_FORWARD2: - { - if( (ctx->speed == 0.0f) && (ctx->desired_speed < 0.0f) )//后退 - { - ctx->state = STATE_BACKWARD; // 零速时允许切换方向 - } - else if ( (ctx->speed == 0.0f) && (ctx->desired_speed > 0.0f) ) //前进 - { - ctx->state = STATE_FORWARD; // 显式保持当前状态 - } - else - { - ctx->state = DEC_FORWARD2; // 新增：其他情况保持前进状态 - *torque = 0;//减速的话先发1，然后再发0 - ctx->desired_speed = 0; - } - 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->state = DEC_BACKWARD; // 显式保持当前状态 - *torque = -1;//倒挡减速的话先发-1，然后再发0 - handle_cnt = 0; - } - else - { - ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态 - } - break; - } - - //------------------------------------------- - // 后退减速状态： - //------------------------------------------- - case DEC_BACKWARD: - { - if (handle_cnt >= 10) - { - *torque = 0;//减速的话先发1，然后再发0 - ctx->state = DEC_BACKWARD2; // 零速时允许切换方向 - handle_cnt = 0; - } - else - { - handle_cnt ++; - ctx->state = DEC_BACKWARD; // 新增：其他情况保持前进状态 - } - break; - } - //------------------------------------------- - // 后退减速状态： - //------------------------------------------- - case DEC_BACKWARD2: - { - if( (ctx->speed == 0.0f) && (ctx->desired_speed < 0.0f) )//后退 - { - ctx->state = STATE_BACKWARD; // 零速时允许切换方向 - } - else if ( (ctx->speed == 0.0f) && (ctx->desired_speed > 0.0f) ) //前进 - { - ctx->state = STATE_FORWARD; // 显式保持当前状态 - } - else - { - ctx->state = DEC_BACKWARD2; // 新增：其他情况保持前进状态 - *torque = 0;//减速的话先发1，然后再发0 - ctx->desired_speed = 0; - } - break; - } - } -} - - - // 差速处理函数 static void diffProcess(void *signal_id) { @@ -601,136 +591,44 @@ static void diffProcess(void *signal_id) // 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度 calculateCurrentState(dt); - // 当速度小于1时，设定为原地转向 - if (fabsf(diff_data.desired_speed) <= 1.0f) + // 当速度小于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; // 原地转向时，设定一个固定的偏航率 + 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; } - - -// if( ( (diff_data.desired_speed > 0) && (diff_data.speed > 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed < 0) ) )//同相 -// { -// if( fabs(diff_data.desired_speed) > fabs(diff_data.speed) )//期望大于当前速度，表示加速，所以用大PID -// { -// setPidParameters(&speed_pid, -// Acc_front_speed_pid.kp, -// Acc_front_speed_pid.ki, -// Acc_front_speed_pid.kd, -// Acc_front_speed_pid.integral_limit, -// Acc_front_speed_pid.output_limit -// ); -// } -// else -// { -// setPidParameters(&speed_pid, -// Dec_front_speed_pid.kp, -// Dec_front_speed_pid.ki, -// Dec_front_speed_pid.kd, -// Dec_front_speed_pid.integral_limit, -// Dec_front_speed_pid.output_limit -// ); -// -// } -// }//反相就先小PID减速 -// else -// { -// setPidParameters(&speed_pid, -// Dec_front_speed_pid.kp, -// Dec_front_speed_pid.ki, -// Dec_front_speed_pid.kd, -// Dec_front_speed_pid.integral_limit, -// Dec_front_speed_pid.output_limit -// ); -// } - - - -// float positive_speed = fabs(diff_data.desired_speed);//取绝对值 20250701 扭矩控制不需要考虑正负 -// -// if(diff_data.desired_speed < 0)//判断方向 小于0就反转，大于0电机正转 -// { -// diff_data.out_motor_dir = 2; -// } -// else if(diff_data.desired_speed > 0) -// { -// diff_data.out_motor_dir = 1; -// } -// else -// { -// diff_data.out_motor_dir = 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(); - -// if( ( (output_speed > 0) && (diff_data.speed < 0) ) || ( (output_speed < 0) && (diff_data.speed > 0) ) )//方向相反不给反方向扭矩，直接清0，给反方向扭矩转速有问题 -// { -// if( ( (diff_data.desired_speed > 0) && (diff_data.speed < 0) ) || ( (diff_data.desired_speed < 0) && (diff_data.speed > 0) ) )//期望和当前速度相反的话，那直接运行反方向扭矩，解决下坡不能减速的问题。 -// { -// -// } -// else -// { -// output_speed = 0; -// resetPidIntegral(&speed_pid); -// } -// } -// - handleVehicleState(&diff_data,&output_speed); //20250704 换挡函数 速度为0才能换挡,溜车需要先 - - - - if( ( output_speed < 0) && (diff_data.desired_speed > 0) && (diff_data.speed > 0) )//解决同向导致电机减速，震荡的问题。同相减速，发1。 - { - output_speed = 1; - } - else if( ( output_speed > 0) && (diff_data.desired_speed < 0) && (diff_data.speed < 0) ) - { - output_speed = -1; - } - else - { - } - - - // 限制输出速度在当前速度和最大加速度计算出来的速度之间 + // 限制输出速度在当前速度和最大加速度计算出来的速度之间 // output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt); -// printf("output_speed: %f, output_yaw: %f\n", output_speed, output_yaw_rate); - -// if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && (diff_data.left_motor_speed < 50.0f) && (diff_data.right_motor_speed > -50.0f))//手柄回中，速度小的时候清0 -// { -// resetPidIntegral(&speed_pid); -// resetPidIntegral(&yaw_rate_pid); -// output_speed = 0; -// output_yaw_rate = 0; -// } -// - - if( (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0 + if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) && ( 10 > fabs(diff_data.left_motor_speed) ) && ( 10 > fabs(diff_data.right_motor_speed) ) )//手柄回中，速度小的时候清0 { resetPidIntegral(&speed_pid); resetPidIntegral(&yaw_rate_pid); output_speed = 0; output_yaw_rate = 0; - } - - // 使用差速车辆动力学模型计算左右电机的期望速度 + } + + // 使用差速车辆动力学模型计算左右电机的期望速度 float out_torque[4] = {0,0,0,0}; + // 使用差速车辆动力学模型计算左右电机的期望速度 computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); -// computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_motor_speed[0]); - - if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 { diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID @@ -743,7 +641,7 @@ static void diffProcess(void *signal_id) } - if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + if( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 { diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID @@ -755,55 +653,68 @@ static void diffProcess(void *signal_id) diff_data.right_diff_touue = 0; } - if(out_torque[0] > 0) + + if(out_torque[0] > 0)//根据大小来限定值为分配扭矩。最小就是0扭矩。 { - diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]); + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]); } else { - diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]); + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]); } if(out_torque[1] > 0) { - diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]); + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]); } else { - diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]); - } - - - diff_data.out_motor_speed[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 - diff_data.out_motor_speed[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]); + } - diff_data.out_motor_speed[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f; - diff_data.out_motor_speed[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f; -// diff_data.out_motor_speed[0] = calculatePidOutput(&Left_front_speed_pid, diff_data.out_left_motor_speed, diff_data.left_front_motor_speed, 0.0f, dt); -// diff_data.out_motor_speed[1] = calculatePidOutput(&right_front_speed_pid, diff_data.out_right_motor_speed, diff_data.right_front_motor_speed, 0.0f, dt); -// diff_data.out_motor_speed[2] = calculatePidOutput(&Left_Rear_speed_pid, diff_data.out_left_motor_speed, diff_data.left_rear_motor_speed, 0.0f, dt); -// diff_data.out_motor_speed[3] = calculatePidOutput(&right_Rear_speed_pid, diff_data.out_right_motor_speed, diff_data.right_rear_motor_speed, 0.0f, dt); + diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue); -// diff_data.out_motor_speed[0] = diff_data.out_left_motor_speed; -// diff_data.out_motor_speed[2] = diff_data.out_left_motor_speed; -// -// diff_data.out_motor_speed[1] = diff_data.out_right_motor_speed; -// diff_data.out_motor_speed[3] = diff_data.out_right_motor_speed; + diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue); + diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue); + + out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩 + out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq); - // 设置电机输出 - setMotorOutput(&diff_data.out_motor_speed[0], - diff_data.max_Torq, +// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral); + + + + // 设置电机输出 + 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_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); } - publishMessage(&diff_data, 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_speed_diff); + un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_speed_diff); + + publishMessage(&diff_data, 1); + } @@ -929,6 +840,8 @@ static void diffProcess(void *signal_id) // 差速输入处理函数 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; @@ -975,193 +888,44 @@ static void diffInput(void *signal_id) } else if( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左侧电机） { - float speed_temp = (float) ( (int16_t)(un_motor_input1.bit_data.speed) ) ; - if(1 == un_motor_input1.bit_data.gear)//判断挡位 + 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 增加偏移量 + + diff_data.left_rear_motor_speed = - diff_data.left_rear_motor_speed;//20250708 增加反相 + + if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速 { - if(speed_temp < 0)//转速小于0 - { - diff_data.left_rear_motor_speed = -speed_temp; - } - else - { - diff_data.left_rear_motor_speed = speed_temp; - } - } - else if(2 == un_motor_input1.bit_data.gear) - { - if(speed_temp < 0)//转速小于0 - { - diff_data.left_rear_motor_speed = speed_temp;//两个负号得正 - } - else - { - diff_data.left_rear_motor_speed = -speed_temp; - } + motor_speed_temp = diff_data.left_front_motor_speed; } else { + motor_speed_temp = diff_data.left_rear_motor_speed; } -// if(1 == diff_data.motor_dir)//1为负数 20250620 修改为判断电机转动方向， 挡位不可靠 -// { -// diff_data.left_rear_motor_speed = -speed_temp; -// } -// else -// { -// diff_data.left_rear_motor_speed = speed_temp; -// } - if(fabs(diff_data.left_rear_motor_speed) < SWITCH_SPEED) - { - diff_data.left_rear_motor_speed = 0; - } - +// diff_data.left_motor_speed = motor_speed_temp; + diff_data.left_motor_speed = 0; - speed_temp = (float) ( (int16_t)(un_motor_input3.bit_data.speed) ) ; - if(1 == un_motor_input3.bit_data.gear)//判断挡位 - { - if(speed_temp < 0)//转速小于0 - { - diff_data.left_front_motor_speed = -speed_temp; - } - else - { - diff_data.left_front_motor_speed = speed_temp; - } - } - else if(2 == un_motor_input3.bit_data.gear) - { - if(speed_temp < 0)//转速小于0 - { - diff_data.left_front_motor_speed = speed_temp;//两个负号得正 - } - else - { - diff_data.left_front_motor_speed = -speed_temp; - } - } - else - { - } - -// if(1 == diff_data.motor_dir)//1为负数 -// { -// diff_data.left_front_motor_speed = -speed_temp; -// } -// else -// { -// diff_data.left_front_motor_speed = speed_temp; -// } - - if(fabs(diff_data.left_front_motor_speed) < SWITCH_SPEED)//死区 - { - diff_data.left_front_motor_speed = 0; - } - - if(fabs(diff_data.left_front_motor_speed) < fabs(diff_data.left_rear_motor_speed) )//取最小值,避免打滑，拉高转速 - { - diff_data.left_motor_speed = diff_data.left_front_motor_speed; - } - else - { - diff_data.left_motor_speed = diff_data.left_rear_motor_speed; - } - -// printf("motor1: %f, motor3: %f\n", diff_temp[0], diff_temp[1]); -// diff_data.left_motor_speed = 0;// 左侧两个电机所以取平均值 - } else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右侧电机） { - - float speed_temp = (float) ( (int16_t)( un_motor_input2.bit_data.speed) ); -// if(1 == diff_data.motor_dir)//1为负数 -// { -// diff_data.right_front_motor_speed = - speed_temp; -// } -// else -// { -// diff_data.right_front_motor_speed = speed_temp; -// } - if(1 == un_motor_input2.bit_data.gear)//判断挡位 + 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)); + + diff_data.right_front_motor_speed = - diff_data.right_front_motor_speed;//20250708 增加反相 + diff_data.right_rear_motor_speed = - diff_data.right_rear_motor_speed;//20250708 增加反相 + + if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速 { - if(speed_temp < 0)//转速小于0 - { - diff_data.right_front_motor_speed = -speed_temp; - } - else - { - diff_data.right_front_motor_speed = speed_temp; - } - } - else if(2 == un_motor_input2.bit_data.gear) - { - if(speed_temp < 0)//转速小于0 - { - diff_data.right_front_motor_speed = speed_temp;//两个负号得正 - } - else - { - diff_data.right_front_motor_speed = -speed_temp; - } + motor_speed_temp = diff_data.right_rear_motor_speed; } else { + motor_speed_temp = diff_data.right_front_motor_speed; } - if(fabs(diff_data.right_front_motor_speed) < SWITCH_SPEED)//死区 - { - diff_data.right_front_motor_speed = 0; - } - - - speed_temp = ( (float) ( (int16_t)(un_motor_input4.bit_data.speed) ) ); -// if(1 == diff_data.motor_dir)//1为负数 -// { -// diff_data.right_rear_motor_speed = - speed_temp; -// } -// else -// { -// diff_data.right_rear_motor_speed = speed_temp; -// } - if(1 == un_motor_input4.bit_data.gear)//判断挡位 - { - if(speed_temp < 0)//转速小于0 - { - diff_data.right_rear_motor_speed = -speed_temp; - } - else - { - diff_data.right_rear_motor_speed = speed_temp; - } - } - else if(2 == un_motor_input4.bit_data.gear) - { - if(speed_temp < 0)//转速小于0 - { - diff_data.right_rear_motor_speed = speed_temp;//两个负号得正 - } - else - { - diff_data.right_rear_motor_speed = -speed_temp; - } - } - else - { - } - if(fabs(diff_data.right_rear_motor_speed) < SWITCH_SPEED)//死区 - { - diff_data.right_rear_motor_speed = 0; - } - if( fabs(diff_data.right_front_motor_speed) < fabs(diff_data.right_rear_motor_speed) )//取最小值,避免打滑，拉高转速 - { - diff_data.right_motor_speed = diff_data.right_front_motor_speed; - } - else - { - diff_data.right_motor_speed = diff_data.right_rear_motor_speed; - } +// diff_data.right_motor_speed = motor_speed_temp; -// diff_data.right_motor_speed = 0;// 左侧两个电机所以取平均值 + diff_data.right_motor_speed = 0; } else{} @@ -1183,44 +947,42 @@ static void diffInput(void *signal_id) 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 = 0;//停车 就为0 -// } -// else -// { - -// } - - diff_data.speed_inter = (uint16_t)(int16_t)speed_pid.integral; - diffProcess(&diff_data);//计算左右电机期望转速 -// printf("dir_state: %d,speed: %f, tou: %f\n", diff_data.state,diff_data.speed,diff_data.out_motor_speed[0]); + if((power_data.current_state == POWER_WORKING))//电机上电才运行 + { + diffProcess(&diff_data);//计算左右电机期望转速 + } + else + { + resetPidIntegral(&speed_pid); + resetPidIntegral(&yaw_rate_pid); + diff_data.motor_state[0] = STATE_INIT; + diff_data.motor_state[1] = STATE_INIT; + diff_data.motor_state[2] = STATE_INIT; + diff_data.motor_state[3] = STATE_INIT; + } publishMessage(&diff_data, 1); } - // 预充完成处理函数 void preChargeFinish(void *signal_id) { (void)signal_id; - - for(uint8_t i = 0; i<4; i++) - { - diff_data.out_motor_speed[i] = 0; - } - setMotorOutput(&diff_data.out_motor_speed[0],(uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr")); + 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; + un_motor_output3.bit_data.gear = 0; // 0表示空挡 + un_motor_output4.bit_data.gear = 0; + publishMessage(&un_motor_output1, 1); publishMessage(&un_motor_output2, 1); + publishMessage(&un_motor_output3, 1); + publishMessage(&un_motor_output4, 1); } @@ -1274,15 +1036,25 @@ void diffParametersInit(void *signal_id) } + 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("LF_speed = %f,RF_speed = %f,LR_speed = %f,RR_speed = %f\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("PID_P: %f, PID_I: %f\n", speed_pid.kp, speed_pid.ki); +// 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]); - printf("left_front: %f, right_front: %f, left_rear: %f, right_rear: %f\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("left_front_tou: %f, right_front_tou: %f, left_rear_tou: %f, right_rear_tou: %f\n", diff_data.out_motor_speed[0], diff_data.out_motor_speed[2],diff_data.out_motor_speed[2], diff_data.out_motor_speed[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_yaw_rate: %f\n", deffspeed, deffcurvature); + printf(" car state = %d,%d,%d,%d\n", diff_data.motor_state[0],diff_data.motor_state[1],diff_data.motor_state[2],diff_data.motor_state[3]); + timerStart(&diff_app_timer,1000,1);//1s调用一次 } diff --git a/app/app_differential_drive.h b/app/app_differential_drive.h index 4c8bec9..1e137c6 100644 --- a/app/app_differential_drive.h +++ b/app/app_differential_drive.h @@ -8,37 +8,34 @@ extern "C" #include "app_config.h" -#define SPEED_MODE 0x0C -#define THROTTLE_MODE 0x05 -#define TORQUE_MODE 0x0A +#define SPEED_MODE 0x01 +#define TORQUE_MODE 0x02 -#define MOTOR_MODE THROTTLE_MODE +#define MOTOR_MODE TORQUE_MODE #define SPEED_PID_MODE 0 #define THROTTLE_PID_MODE 1 - - - +#define TORQUE_HYSTERESIS_THRESHOLD 0.3f #define SWITCH_SPEED 20.0f - #define DIFF_SPEED 20.0f +// 状态机内部状态 typedef enum { STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定） - STATE_FORWARD, ///< 正向旋转状态 - STATE_BACKWARD, ///< 反向旋转状态 - DEC_FORWARD, ///< 正向减速状态 - DEC_FORWARD2, ///< 正向溜坡状态 - DEC_BACKWARD, ///< 反向减速状态 - DEC_BACKWARD2 ///< 反向溜坡状态 - + STATE_FORWARD, ///< 前进 + STATE_BACKWARD, ///< 后退 } MotorState; +typedef enum +{ + MODE_MANUAL, // 手动模式 + MODE_AUTO // 自动模式 +} ControlMode; // 状态机内部状态 typedef enum @@ -55,67 +52,47 @@ typedef enum } MotorDir; - -typedef enum -{ - MODE_MANUAL, // 手动模式 - MODE_AUTO // 自动模式 -} ControlMode; - typedef struct DiffData { ControlMode mode ; // 控制模式 - MotorState state; //当前状态机状态 - MotorDir motor_dir; // 当前车辆方向 - DirState dir_state; - - float desired_speed; // 期望速度 + MotorState motor_state[4]; //当前车辆状态 + float desired_speed; // 期望速度 float desired_curvature; // 期望曲率 float left_motor_speed; // 当前左电机速度 float right_motor_speed; // 当前右电机速度 - - float left_front_motor_speed; // 当前左前电机速度 - float right_front_motor_speed; // 当前右前电机速度 - float left_rear_motor_speed; // 当前左后电机速度 - float right_rear_motor_speed; // 当前右后电机速度 - - uint8_t out_motor_dir; // 电机状态 - - float speed_last; // 速度过去值 - float torq_last; // 扭矩过去值 - uint8_t counter; // 方向判断计数器 + float left_front_motor_speed; // 当前左前电机速度 + float right_front_motor_speed; // 当前右前电机速度 + float left_rear_motor_speed; // 当前左后电机速度 + float right_rear_motor_speed; // 当前右后电机速度 + float speed; // 当前车速 + float curvature; // 当前曲率 + float yaw_rate; // 当前角速度 + float desired_yaw_rate; // 期望角速度 + float acceleration; // 当前加速度 + float deceleration; // 当前减速度 + float max_speed; // 最大速度 + float desired_acceleration; // 期望加速度 + float desired_deceleration; // 期望减速度 + uint8_t emergency_stop_switch; // 急停开关 + uint8_t remote_emergency_stop; // 遥控器急停开关 + uint8_t emergency_stop_state; // 急停状态 + float out_left_motor_speed; // 输出左电机速度 + float out_right_motor_speed; // 输出右电机速度 + float out_torq[4]; //4个电机扭矩 + float max_Torq; // 最大扭矩限制 + float min_Torq; // 最小扭矩限制 float left_speed_diff; // 左侧转速差 float right_speed_diff; // 右侧转速差 float left_diff_touue; // 左侧扭矩差 - float right_diff_touue; // 右侧扭矩差 - + float right_diff_touue; // 右侧扭矩差 float diff_dead_zone; // 差速速度死区 - - - float out_motor_speed[4]; - float speed; // 当前车速 - float curvature; // 当前曲率 - float yaw_rate; // 当前角速度 - float desired_yaw_rate; // 期望角速度 - float acceleration; // 当前加速度 - float deceleration; // 当前减速度 - float max_speed; // 最大速度 - float desired_acceleration; // 期望加速度 - float desired_deceleration; // 期望减速度 - uint8_t emergency_stop_switch; // 急停开关 - uint8_t remote_emergency_stop; // 遥控器急停开关 - uint8_t emergency_stop_state; // 急停状态 - float out_left_motor_speed; // 输出左电机速度 - float out_right_motor_speed; // 输出右电机速度 - uint16_t max_Torq; // 最大扭矩限制 - uint16_t speed_inter; - } DiffData; + // 声明外部变量 extern DiffData diff_data; diff --git a/app/app_request.c b/app/app_request.c index 5390740..c80ae65 100644 --- a/app/app_request.c +++ b/app/app_request.c @@ -15,7 +15,6 @@ static void processRequestframe(uint16_t id) static uint8_t VehicleStaACC2 = 0; static uint8_t VehicleStaACC3 = 0; - uint16_t RgExchangeTemp = 0; uint32_t Rg32ExchangeTemp = 0; //----------------------------------------------------------- // printf("request_read_id:%d\n",id); @@ -43,7 +42,7 @@ static void processRequestframe(uint16_t id) un_motor_status_output.bit_data.frame_type = 0x2100;//帧类型 un_motor_status_output.bit_data.frame_length = 0x1E00;//帧长 un_motor_status_output.bit_data.accumulated = VehicleStaACC1++;//累加值 - RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ; +// RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ; // un_motor_status_output.bit_data.left_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//左侧扭矩限制 // un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//右侧扭矩限制 // un_motor_status_output.bit_data.left_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//左侧馈电功率 @@ -253,20 +252,20 @@ static void requestInput(void *signal_id) request16_temp = (uint16_t)(int16_t)(diff_data.right_diff_touue * 10000.0); un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率 - request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[0]);// + request16_temp = (uint16_t)(int16_t)(diff_data.left_front_motor_speed);// un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//左前 - request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[1]);//out_right_motor_speed + request16_temp = (uint16_t)(int16_t)(diff_data.right_front_motor_speed);//out_right_motor_speed un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//右前 - request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[2]);// + request16_temp = (uint16_t)(int16_t)(diff_data.left_rear_motor_speed);// un_motor_status_output.bit_data.left_power_in = ((request16_temp << 8) | (request16_temp >> 8));//左后 - request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[3]);//out_right_motor_speed + request16_temp = (uint16_t)(int16_t)(diff_data.right_rear_motor_speed);//out_right_motor_speed un_motor_status_output.bit_data.right_power_in = ((request16_temp << 8) | (request16_temp >> 8));//右后 - un_vehicle_Info_output.bit_data.vehicle_fault_state = diff_data.state; - un_vehicle_Info_output.bit_data.battery_soc = diff_data.motor_dir; + un_vehicle_Info_output.bit_data.vehicle_fault_state = diff_data.motor_state[0]; + un_vehicle_Info_output.bit_data.battery_soc = diff_data.motor_state[1]; request16_temp = (uint16_t)(int16_t)(diff_data.left_rear_motor_speed + 30000); un_motor_status_output.bit_data.left_wheel_speed = ((request16_temp << 8) | (request16_temp >> 8));;//左侧后轮速 diff --git a/interface.c b/interface.c index 810c118..4ce8d86 100644 --- a/interface.c +++ b/interface.c @@ -7,6 +7,15 @@ UnMotorInput un_motor_input2 ;//电机控制器2 UnMotorInput un_motor_input3 ;//电机控制器1 左后侧 UnMotorInput un_motor_input4 ;//电机控制器2 右后侧 + +UnMotorTempInput un_motor_temp1 ;//电机控制器1 左前侧 +UnMotorTempInput un_motor_temp2 ;//电机控制器2 右前侧 +UnMotorTempInput un_motor_temp3 ;//电机控制器3 左后侧 +UnMotorTempInput un_motor_temp4 ;//电机控制器4 右后侧 + + + + UnBmsInput un_bms_input ;//BMS接收数据 UnTempModuleInput un_temp_module_input ;//温度采集模块 @@ -69,6 +78,7 @@ UnAutoControlOutput un_auto_control_output ;// 自动控制数 +UnCanDebugOutput un_can_debug_output;//调试输出 diff --git a/interface.h b/interface.h index f693285..336edeb 100644 --- a/interface.h +++ b/interface.h @@ -22,38 +22,57 @@ #define NORMAL 1 +//-----CAN---------------------------------------------------------------- +// 接收电机控制器输入 +//typedef struct _StrMotorInput +//{ +////-----接收数据0x10F90708---------------------------------------------- +// unsigned int control_data3 : 16; // 读取的控制数据 +// unsigned int torque : 16; // 当前扭矩 +// unsigned int phase_current : 16; // 当前相电流 +// unsigned int speed : 16; // 当前转速 +// +// unsigned int gear : 2; // 反馈档位 +// unsigned int speed_gear : 3; // 当前扭矩 +// unsigned int reserve1 : 1; // 保留 +// unsigned int reserve2 : 1; // 保留 +// unsigned int reserve3 : 1; // 保留 +// +// unsigned int reserve4 : 8; // 保留 +// +// unsigned int ready : 1; // Bit0: READY, 0-有告警不能行驶, 1-工作正常可行驶 +// unsigned int reserved1 : 1; // Bit1: ECO状态（预留） +// unsigned int eabs : 1; // Bit2: EABS, 0-停止或驱动行驶, 1-能量回收状态 +// unsigned int boost : 1; // Bit3: BOOST状态, 0-正常驱动, 1-BOOST驱动 +// unsigned int reserved2 : 1; // Bit4: 预留 +// unsigned int rotation_dir : 1; // Bit5: 转动方向, 0-电机逆时针转, 1-电机顺时针转 +// unsigned int motor_dir : 1; // Bit6: 电机方向, 0-电机右置, 1-电机左置 +// unsigned int main_relay : 1; // Bit7: 主继电器, 0-断开, 1-闭合 +// +// unsigned int reserve5 : 16; // 保留 +// unsigned int reserve6 : 16; // 保留 +// unsigned int reserve7 : 16; // 保留 +//} StrMotorInput; +// +//typedef union _UnMotorInput +//{ +// StrMotorInput bit_data; // 使用定义的结构体变量名 +// uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 +//} UnMotorInput; + + //-----CAN---------------------------------------------------------------- // 接收电机控制器输入 typedef struct _StrMotorInput { -//-----接收数据0x10F90708---------------------------------------------- - unsigned int control_data3 : 16; // 读取的控制数据 - unsigned int torque : 16; // 当前扭矩 - unsigned int phase_current : 16; // 当前相电流 - unsigned int speed : 16; // 当前转速 + //-----接收数据0x101或者0x201---------------------------------------------- + unsigned int speed : 16; // 转速 偏移量 -30000 + unsigned int bus_voltage : 16; // 母线电压 系数 0.1 偏移量 -3000 + unsigned int torque : 16; // 扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int fault_code : 8; // 故障码 + unsigned int heartbeat : 8; // 心跳 - unsigned int gear : 2; // 反馈档位 - unsigned int speed_gear : 3; // 当前扭矩 - unsigned int reserve1 : 1; // 保留 - unsigned int reserve2 : 1; // 保留 - unsigned int reserve3 : 1; // 保留 - - unsigned int reserve4 : 8; // 保留 - - unsigned int ready : 1; // Bit0: READY, 0-有告警不能行驶, 1-工作正常可行驶 - unsigned int reserved1 : 1; // Bit1: ECO状态（预留） - unsigned int eabs : 1; // Bit2: EABS, 0-停止或驱动行驶, 1-能量回收状态 - unsigned int boost : 1; // Bit3: BOOST状态, 0-正常驱动, 1-BOOST驱动 - unsigned int reserved2 : 1; // Bit4: 预留 - unsigned int rotation_dir : 1; // Bit5: 转动方向, 0-电机逆时针转, 1-电机顺时针转 - unsigned int motor_dir : 1; // Bit6: 电机方向, 0-电机右置, 1-电机左置 - unsigned int main_relay : 1; // Bit7: 主继电器, 0-断开, 1-闭合 - - unsigned int reserve5 : 16; // 保留 - unsigned int reserve6 : 16; // 保留 - unsigned int reserve7 : 16; // 保留 } StrMotorInput; - typedef union _UnMotorInput { StrMotorInput bit_data; // 使用定义的结构体变量名 @@ -62,10 +81,43 @@ typedef union _UnMotorInput +// 接收电机控制器输入 +typedef struct _StrCanDebugOutput +{ + uint8_t speed; + uint8_t desired_speed; + uint8_t curvature; + uint8_t desired_curvature; + uint16_t set_left_out; + uint16_t set_right_out; +} StrCanDebugOutput; + +typedef union _UnCanDebugOutput +{ + StrCanDebugOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrCanDebugOutput)]; // 通过结构体类型确定大小 +} UnCanDebugOutput; +typedef struct _StrMotorTempInput +{ + //-----接收数据0x103或者0x104---------------------------------------------- + unsigned int controller_temp : 8; // 控制器温度 偏移量 -40 + unsigned int motor_temp : 8; // 电机温度 偏移量 -40 + unsigned int motor_max_torque: 16; // 电机最大扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int reserved_1 : 8; // 保留1 + unsigned int reserved_2 : 8; // 保留2 + unsigned int reserved_3 : 8; // 保留3 + unsigned int heartbeat2 : 8; // 心跳 + +} StrMotorTempInput; +typedef union _UnMotorTempInput +{ + StrMotorTempInput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorInput)]; // 通过结构体类型确定大小 +} UnMotorTempInput; @@ -280,27 +332,27 @@ typedef union _UnSwSample //-----输出数据结构--------------------------------------------------------------- //-----CAN---------------------------------------------------------------- // 输出到电机控制器 -//typedef struct _StrMotorOutput -//{ -////-----发送数据0x201或者0x202---------------------------------------------- -// unsigned int mode : 8; // 模式 0x1恒速模式，0x2恒扭模式，其他无效 -// unsigned int gear : 8; // 档位 0x0空挡模式，0x1前进挡，0x2倒退档，其他无效 -// unsigned int set_torque : 16; // 给定扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 -// unsigned int set_rotation_speed : 16; // 给定转速 偏移量 -30000 -// unsigned int fault_code : 8; // 故障码 -// unsigned int heartbeat : 8; // 心跳 -////-----发送数据0x401或者0x402---------------------------------------------- -// unsigned int feed_power : 16; // 馈电功率 单位为 W 最大为10KW -// unsigned int discharge_power : 16; // 放电功率 单位为 W 最大为15kW -// unsigned int reserve1 : 16; // 保留 -// unsigned int reserve2 : 16; // 保留 -//} StrMotorOutput; -// -//typedef union _UnMotorOutput -//{ -// StrMotorOutput bit_data; // 使用定义的结构体变量名 -// uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 -//} UnMotorOutput; +typedef struct _StrMotorOutput +{ +//-----发送数据0x201或者0x202---------------------------------------------- + unsigned int mode : 8; // 模式 0x1恒速模式，0x2恒扭模式，其他无效 + unsigned int gear : 8; // 档位 0x0空挡模式，0x1前进挡，0x2倒退档，其他无效 + unsigned int set_torque : 16; // 给定扭矩 系数 0.01 偏移量 -300 实际物理量＝数据×系数＋偏移量 + unsigned int set_rotation_speed : 16; // 给定转速 偏移量 -30000 + unsigned int fault_code : 8; // 故障码 + unsigned int heartbeat : 8; // 心跳 +//-----发送数据0x401或者0x402---------------------------------------------- + unsigned int feed_power : 16; // 馈电功率 单位为 W 最大为10KW + unsigned int discharge_power : 16; // 放电功率 单位为 W 最大为15kW + unsigned int reserve1 : 16; // 保留 + unsigned int reserve2 : 16; // 保留 +} StrMotorOutput; + +typedef union _UnMotorOutput +{ + StrMotorOutput bit_data; // 使用定义的结构体变量名 + uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 +} UnMotorOutput; @@ -357,50 +409,50 @@ typedef union _UnGatherOutput -typedef struct _StrMotorOutput -{ -//-----发送数据0x10F80807---------------------------------------------- - unsigned int gear : 2; // 0 表示空挡，1 表示前进，2 表示后退 - unsigned int can_gear : 1; // 0-无效 1-有效。无效时 BIT10 的挡位无效，以线路控制挡位为准 - unsigned int can_break : 1; // 0:不刹车，1-刹车(刹车时才能启动电子刹车) - unsigned int reserve1 : 1; // 0-只有高速档 1-可以通过接线切换 123 档。 - unsigned int reserve2 : 1; // 0-无效 1-有效：线控防盗使能 - unsigned int reserve3 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve4 : 1; // 0-无效 1-有效：线控座桶使能 - - unsigned int reserve5 : 1; // 0-无效 1-有效：线控边撑使能 - unsigned int reserve6 : 1; // 0-无效 1-有效：线控防盗使能 - unsigned int reserve7 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve8 : 1; // 0-无效 1-有效：线控座桶使能 - unsigned int reserve9 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve10 : 1; // 0-无效 1-有效：线控座桶使能 - unsigned int reserve11 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve12 : 1; // 0-无效 1-有效：线控座桶使能 - - unsigned int reserve13 : 1; // 0-无效 1-有效：线控边撑使能 - unsigned int reserve14 : 1; // 0-无效 1-有效：线控防盗使能 - unsigned int reserve15 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve16 : 1; // 0-无效 1-有效：线控座桶使能 - unsigned int reserve17 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve18 : 1; // 0-无效 1-有效：线控座桶使能 - unsigned int motor_direction : 1; // 0 和 1 （静止空闲状态设置有效，在油门、 转矩、转速模式下允许设置，否则使用控制器 内部保存的参数控制电机方向） - unsigned int Contactor : 1; // 0-断开 1-闭合，（部分控制器支持） - - unsigned int reserve19 : 1; // 0-无效 1-有效：线控边撑使能 - unsigned int reserve20 : 1; // 0-无效 1-有效：线控防盗使能 - unsigned int reserve21 : 1; // 0-无效 1-有效：线控巡航使能 - unsigned int reserve22 : 1; // 0-无效 1-有效：线控座桶使能 - unsigned int mode : 4; //0x0:转把控制，0x5:油门模式，0xA:转矩模式， 0xC:转速模式，其它无效停机。在这三种模式下，电机方向设置有效。只允许在停机状态下设置控制模式。 - - unsigned int control_data1 : 16; // 油门-256~+256：(0~+256 表示最大油门,-256 表 示最大电子刹车力度。转矩 -256~10000，（正数 驱 动 扭 矩 0~1000.0Nm，负数表示刹车，-256 表示最大电子刹车力度）转速-256~12000,（正数表示转速 0~12000rpm， 负数表示刹车，-256 最大刹车力度） - unsigned int control_data2 : 16; // 控制数据和上一个一样 -} StrMotorOutput; - -typedef union _UnMotorOutput -{ - StrMotorOutput bit_data; // 使用定义的结构体变量名 - uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 -} UnMotorOutput; +//typedef struct _StrMotorOutput +//{ +////-----发送数据0x10F80807---------------------------------------------- +// unsigned int gear : 2; // 0 表示空挡，1 表示前进，2 表示后退 +// unsigned int can_gear : 1; // 0-无效 1-有效。无效时 BIT10 的挡位无效，以线路控制挡位为准 +// unsigned int can_break : 1; // 0:不刹车，1-刹车(刹车时才能启动电子刹车) +// unsigned int reserve1 : 1; // 0-只有高速档 1-可以通过接线切换 123 档。 +// unsigned int reserve2 : 1; // 0-无效 1-有效：线控防盗使能 +// unsigned int reserve3 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve4 : 1; // 0-无效 1-有效：线控座桶使能 +// +// unsigned int reserve5 : 1; // 0-无效 1-有效：线控边撑使能 +// unsigned int reserve6 : 1; // 0-无效 1-有效：线控防盗使能 +// unsigned int reserve7 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve8 : 1; // 0-无效 1-有效：线控座桶使能 +// unsigned int reserve9 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve10 : 1; // 0-无效 1-有效：线控座桶使能 +// unsigned int reserve11 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve12 : 1; // 0-无效 1-有效：线控座桶使能 +// +// unsigned int reserve13 : 1; // 0-无效 1-有效：线控边撑使能 +// unsigned int reserve14 : 1; // 0-无效 1-有效：线控防盗使能 +// unsigned int reserve15 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve16 : 1; // 0-无效 1-有效：线控座桶使能 +// unsigned int reserve17 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve18 : 1; // 0-无效 1-有效：线控座桶使能 +// unsigned int motor_direction : 1; // 0 和 1 （静止空闲状态设置有效，在油门、 转矩、转速模式下允许设置，否则使用控制器 内部保存的参数控制电机方向） +// unsigned int Contactor : 1; // 0-断开 1-闭合，（部分控制器支持） +// +// unsigned int reserve19 : 1; // 0-无效 1-有效：线控边撑使能 +// unsigned int reserve20 : 1; // 0-无效 1-有效：线控防盗使能 +// unsigned int reserve21 : 1; // 0-无效 1-有效：线控巡航使能 +// unsigned int reserve22 : 1; // 0-无效 1-有效：线控座桶使能 +// unsigned int mode : 4; //0x0:转把控制，0x5:油门模式，0xA:转矩模式， 0xC:转速模式，其它无效停机。在这三种模式下，电机方向设置有效。只允许在停机状态下设置控制模式。 +// +// unsigned int control_data1 : 16; // 油门-256~+256：(0~+256 表示最大油门,-256 表 示最大电子刹车力度。转矩 -256~10000，（正数 驱 动 扭 矩 0~1000.0Nm，负数表示刹车，-256 表示最大电子刹车力度）转速-256~12000,（正数表示转速 0~12000rpm， 负数表示刹车，-256 最大刹车力度） +// unsigned int control_data2 : 16; // 控制数据和上一个一样 +//} StrMotorOutput; +// +//typedef union _UnMotorOutput +//{ +// StrMotorOutput bit_data; // 使用定义的结构体变量名 +// uint8_t arr[sizeof(StrMotorOutput)]; // 通过结构体类型确定大小 +//} UnMotorOutput; @@ -865,6 +917,11 @@ extern UnMotorOutput un_motor_output1; //电机输 extern UnMotorOutput un_motor_output2; //电机输出 extern UnMotorOutput un_motor_output3; //电机输出 extern UnMotorOutput un_motor_output4; //电机输出 +extern UnMotorTempInput un_motor_temp1; //电机控制器1 温度 +extern UnMotorTempInput un_motor_temp2; //电机控制器2 温度 +extern UnMotorTempInput un_motor_temp3; //电机控制器3 左后侧 +extern UnMotorTempInput un_motor_temp4; //电机控制器4 右后侧 + extern UnInfCanKGFOutput un_inf_can_kgf_output1; extern UnInfCanKGFOutput un_inf_can_kgf_output2; extern UnHBridgeOutput un_h_bridge_output; @@ -909,7 +966,7 @@ extern UnSdoOutput un_sdo_output ;//转向电机输出 //变量 extern uint8_t test_app[26]; - +extern UnCanDebugOutput un_can_debug_output;//调试输出 //函数 diff --git a/interface_can.c b/interface_can.c index cd9b72f..ce42385 100644 --- a/interface_can.c +++ b/interface_can.c @@ -287,8 +287,7 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle, { flexcan_frame_t *buf = (flexcan_frame_t *)userData; - uint8_t i = 0; - static uint32_t start_time = 0; + uint8_t i = 0; //-------------------------------------------------------------- switch (status) { @@ -296,51 +295,45 @@ void flexcan_Receive_callback_1(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if(MOTOR_INPUT_ID_1 == (buf->id))//1 - { - time_elapsed = getCurrentTime() - start_time; - start_time = getCurrentTime(); - + if(LEFT_FRONT_MOTOR_INPUT1 == (buf->id)) + { can_fault_info.bit_data.motor1_count ++; for(i = 0; i < (buf->length); i++) { un_motor_input1.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input1, 1); - -// CAN_Send_Msg(&can_handle_1, 0x11111111, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input1, 8, 18);// - } - else if(MOTOR_INPUT_ID_3 == (buf->id)) + publishMessage(&un_motor_input1, 1); + } + else if( LEFT_FRONT_MOTOR_INPUT2 == (buf->id) ) { -// can_fault_info.bit_data.motor1_count ++; + for(i = 0; i < (buf->length); i++) + { + un_motor_temp1.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp1, 1);//ź + } + else if( LEFT_REAR_MOTOR2_INPUT1 == (buf->id) )// + { + can_fault_info.bit_data.motor3_count ++; for(i = 0; i < (buf->length); i++) { un_motor_input3.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input3, 1); -// CAN_Send_Msg(&can_handle_1, 0x11111113, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input3, 8, 18);// - } - else if(MOTOR_INPUT_ID_5 == (buf->id))//յλ +// publishMessage(&un_motor_input3, 1); //޸Ϊ + } + else if( LEFT_REAR_MOTOR2_INPUT2 == (buf->id) ) { for(i = 0; i < (buf->length); i++) { - un_motor_input1.arr[i+8] = buf->dataBuffer[i]; - } -// CAN_Send_Msg(&can_handle_1, 0x11111115, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input1.arr[8], 8, 18);// - - } - else if(MOTOR_INPUT_ID_7 == (buf->id))//յλ - { - for(i = 0; i < (buf->length); i++) - { - un_motor_input3.arr[i+8] = buf->dataBuffer[i]; - -// CAN_Send_Msg(&can_handle_1, 0x11111117, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input3.arr[8], 8, 18);// - } + un_motor_temp3.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp3, 1);//ź } - else{} + else{} break; case FLEXCAN_TX_IDLE: @@ -387,55 +380,45 @@ void flexcan_Receive_callback_2(flexcan_handle_t *handle, break; case FLEXCAN_RX_FIFO_IDLE: - if(MOTOR_INPUT_ID_2 == (buf->id))//2 - { - time_elapsed1 = getCurrentTime() - start_time1; - start_time1 = getCurrentTime(); - + if(RIGHT_FRONT_MOTOR_INPUT1 == (buf->id)) + { can_fault_info.bit_data.motor2_count ++; for(i = 0; i < (buf->length); i++) { un_motor_input2.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input2, 1); - -// CAN_Send_Msg(&can_handle_2, 0x11111112, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input2, 8, 18);// - - } - else if(MOTOR_INPUT_ID_4 == (buf->id)) + publishMessage(&un_motor_input2, 1); + } + else if( RIGHT_FRONT_MOTOR_INPUT2 == (buf->id) ) { - + for(i = 0; i < (buf->length); i++) + { + un_motor_temp2.arr[i] = buf->dataBuffer[i]; + } + //ź + publishMessage(&un_motor_temp2, 1);//ź + } + else if( RIGHT_REAR_MOTOR_INPUT1 == (buf->id) )// + { + can_fault_info.bit_data.motor4_count ++; for(i = 0; i < (buf->length); i++) { un_motor_input4.arr[i] = buf->dataBuffer[i]; } //ź - publishMessage(&un_motor_input4, 1); - -// CAN_Send_Msg(&can_handle_2, 0x11111114, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input4, 8, 18);// - - } - else if(MOTOR_INPUT_ID_6 == (buf->id))//յλ + publishMessage(&un_motor_input4, 1); + } + else if( RIGHT_REAR_MOTOR_INPUT2 == (buf->id) ) { for(i = 0; i < (buf->length); i++) { - un_motor_input2.arr[i+8] = buf->dataBuffer[i]; - } - -// CAN_Send_Msg(&can_handle_2, 0x11111116, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input2.arr[8], 8, 18);// - - } - else if(MOTOR_INPUT_ID_8 == (buf->id))//յλ - { - for(i = 0; i < (buf->length); i++) - { - un_motor_input4.arr[i+8] = buf->dataBuffer[i]; + un_motor_temp4.arr[i] = buf->dataBuffer[i]; } -// CAN_Send_Msg(&can_handle_2, 0x11111118, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_input4.arr[8], 8, 18);// - - } - else{} + //ź + publishMessage(&un_motor_temp4, 1);//ź + } + else{} break; case FLEXCAN_TX_IDLE: @@ -1135,18 +1118,35 @@ static void processSdoOutput8(void *signal_id) static void processMotorOutput1(void *signal_id) { - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_1, 0x10F81807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת - CAN_Send_Msg(&can_handle_1, 0x10F83807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);// + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1, 8, 15);//1Ťغת + CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3, 8, 16);//1Ťغת } static void processMotorOutput2(void *signal_id) { - (void)signal_id; // DZΪʹã - CAN_Send_Msg(&can_handle_2, 0x10F82807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ť ת - CAN_Send_Msg(&can_handle_2, 0x10F84807, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);// + (void)signal_id; // DZΪʹã + CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2, 8, 15);//2Ťغת + CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT1, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4, 8, 16);//2Ťغת } +//static void processMotorOutput3(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// +// CAN_Send_Msg(&can_handle_1, LEFT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output1.arr[8], 8, 17);//1 +// CAN_Send_Msg(&can_handle_1, LEFT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output3.arr[8], 8, 18);//1 +// +//} +// +//static void processMotorOutput4(void *signal_id) +//{ +// (void)signal_id; // DZΪʹã +// CAN_Send_Msg(&can_handle_2, RIGHT_FRONT_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output2.arr[8], 8, 17);//2 +// CAN_Send_Msg(&can_handle_2, RIGHT_REAR_MOTOR_OUTPUT2, FLEXCAN_STANDARD_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_motor_output4.arr[8], 8, 18);//2 +//} + + static void processKgfOutput1(void *signal_id) { @@ -1342,33 +1342,33 @@ void canInterfaceInit(void) memset(&un_motor_output1, 0, sizeof(UnMotorOutput)); memset(&un_motor_output2, 0, sizeof(UnMotorOutput)); - un_motor_output1.bit_data.mode = 0x05; - un_motor_output1.bit_data.gear = 0x01; - un_motor_output1.bit_data.can_gear = 0x01; - un_motor_output1.bit_data.motor_direction = 0x01; - un_motor_output1.bit_data.control_data1 = 0x0; - un_motor_output1.bit_data.control_data2 = 0x0; - - un_motor_output2.bit_data.mode = 0x05; - un_motor_output2.bit_data.gear = 0x01; - un_motor_output2.bit_data.can_gear = 0x01; - un_motor_output2.bit_data.motor_direction = 0x00; - un_motor_output2.bit_data.control_data1 = 0x0; - un_motor_output2.bit_data.control_data2 = 0x0; - - un_motor_output3.bit_data.mode = 0x05; - un_motor_output3.bit_data.gear = 0x01; - un_motor_output3.bit_data.can_gear = 0x01; - un_motor_output3.bit_data.motor_direction = 0x01; - un_motor_output3.bit_data.control_data1 = 0x0; - un_motor_output3.bit_data.control_data2 = 0x0; - - un_motor_output4.bit_data.mode = 0x05; - un_motor_output4.bit_data.gear = 0x01; - un_motor_output4.bit_data.can_gear = 0x01; - un_motor_output4.bit_data.motor_direction = 0x00; - un_motor_output4.bit_data.control_data1 = 0x0; - un_motor_output4.bit_data.control_data2 = 0x0; +// un_motor_output1.bit_data.mode = 0x05; +// un_motor_output1.bit_data.gear = 0x01; +// un_motor_output1.bit_data.can_gear = 0x01; +// un_motor_output1.bit_data.motor_direction = 0x01; +// un_motor_output1.bit_data.control_data1 = 0x0; +// un_motor_output1.bit_data.control_data2 = 0x0; +// +// un_motor_output2.bit_data.mode = 0x05; +// un_motor_output2.bit_data.gear = 0x01; +// un_motor_output2.bit_data.can_gear = 0x01; +// un_motor_output2.bit_data.motor_direction = 0x00; +// un_motor_output2.bit_data.control_data1 = 0x0; +// un_motor_output2.bit_data.control_data2 = 0x0; +// +// un_motor_output3.bit_data.mode = 0x05; +// un_motor_output3.bit_data.gear = 0x01; +// un_motor_output3.bit_data.can_gear = 0x01; +// un_motor_output3.bit_data.motor_direction = 0x01; +// un_motor_output3.bit_data.control_data1 = 0x0; +// un_motor_output3.bit_data.control_data2 = 0x0; +// +// un_motor_output4.bit_data.mode = 0x05; +// un_motor_output4.bit_data.gear = 0x01; +// un_motor_output4.bit_data.can_gear = 0x01; +// un_motor_output4.bit_data.motor_direction = 0x00; +// un_motor_output4.bit_data.control_data1 = 0x0; +// un_motor_output4.bit_data.control_data2 = 0x0; diff --git a/interface_can.h b/interface_can.h index a761afa..fb81caa 100644 --- a/interface_can.h +++ b/interface_can.h @@ -20,14 +20,33 @@ #define TX_MB_INDEX (USED_MB_FOR_FIFO) +#define LEFT_FRONT_MOTOR_INPUT1 0x101 +#define LEFT_FRONT_MOTOR_INPUT2 0x103//¶ -//#define MOTOR_INPUT_ID_1 0x101 -//#define MOTOR_INPUT_ID_2 0x102 -#define MOTOR_INPUT_ID_1 0x10F91708//ǰ ת -#define MOTOR_INPUT_ID_2 0x10F92708//ǰ -#define MOTOR_INPUT_ID_3 0x10F93708// -#define MOTOR_INPUT_ID_4 0x10F94708//Һ +#define LEFT_REAR_MOTOR2_INPUT1 0x102 +#define LEFT_REAR_MOTOR2_INPUT2 0x104//¶ + + +#define RIGHT_FRONT_MOTOR_INPUT1 0x101 +#define RIGHT_FRONT_MOTOR_INPUT2 0x103//¶ + + +#define RIGHT_REAR_MOTOR_INPUT1 0x102 +#define RIGHT_REAR_MOTOR_INPUT2 0x104//¶ + + +#define LEFT_FRONT_MOTOR_OUTPUT1 0x201 +#define LEFT_FRONT_MOTOR_OUTPUT2 0x401 + +#define LEFT_REAR_MOTOR_OUTPUT1 0x202 +#define LEFT_REAR_MOTOR_OUTPUT2 0x402 + +#define RIGHT_FRONT_MOTOR_OUTPUT1 0x201 +#define RIGHT_FRONT_MOTOR_OUTPUT2 0x401 + +#define RIGHT_REAR_MOTOR_OUTPUT1 0x202 +#define RIGHT_REAR_MOTOR_OUTPUT2 0x402 #define MOTOR_INPUT_ID_5 0x10F81708//ǰ λ @@ -73,7 +92,10 @@ typedef struct _StrCanFault { uint8_t navigator_count; //Ǽ uint8_t motor1_count; //ֶݼ - uint8_t motor2_count; //ֶݼ + uint8_t motor2_count; //ֶݼ + uint8_t motor3_count; //ֶݼ + uint8_t motor4_count; //ֶݼ + uint8_t bms_count; //bms uint8_t temperature_count; //¶ȼ uint8_t remote_count; //ңؼ @@ -81,6 +103,8 @@ typedef struct _StrCanFault uint8_t navigator_state; //״̬ uint8_t motor1_state; //1״̬ uint8_t motor2_state; //2״̬ + uint8_t motor3_state; //1״̬ + uint8_t motor4_state; //2״̬ uint8_t bms_state; //bms״̬ uint8_t temperature_state; //¶ȼ uint8_t remote_state; //ң״̬ @@ -95,7 +119,6 @@ typedef union _UnCanFault - // canڵ typedef struct _StrCanBuffer {