扭矩模式，增加同侧差速PID
This commit is contained in:
479
app/app_brake.c
479
app/app_brake.c
@@ -1,241 +1,238 @@
|
||||
#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"
|
||||
|
||||
#include "app_brake.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setBrakeOn(void) { return 1; }
|
||||
static inline uint8_t setBrakeOff(void) { return 0; }
|
||||
|
||||
BrakeSystem brake_data;
|
||||
|
||||
// 判断是否需要刹车
|
||||
static uint8_t shouldApplyBrake()
|
||||
{
|
||||
return (brake_data.emergency_stop_switch ||
|
||||
brake_data.remote_emergency_stop ||
|
||||
(brake_data.mode_signal == 0 && brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
|
||||
}
|
||||
|
||||
// 判断是否需要释放刹车
|
||||
static uint8_t shouldReleaseBrake()
|
||||
{
|
||||
return (!brake_data.emergency_stop_switch &&
|
||||
!brake_data.remote_emergency_stop &&
|
||||
((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 输出处理函数
|
||||
static void brakeOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// 根据电机状态，填充发送数据结构，发送信号
|
||||
switch (brake_data.brake_motor_state)
|
||||
{
|
||||
case 1: // 电机前进状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
printf("Brake: Motor forward\n");
|
||||
break;
|
||||
|
||||
case 2: // 电机后退状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();
|
||||
printf("Brake: Motor reverse\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
printf("Brake: Motor off\n");
|
||||
break;
|
||||
}
|
||||
|
||||
publishMessage(&un_h_bridge_output, 1);
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改刹车定时器处理函数
|
||||
static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
//#ifdef OIL_BRAKE
|
||||
switch (brake_data.state)
|
||||
{
|
||||
case BRAKE_STATE_IDLE:
|
||||
if (shouldApplyBrake())
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
|
||||
brake_data.brake_motor_state = 1;
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_BRAKE_ON:
|
||||
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
|
||||
brake_data.brake_motor_state = 2;
|
||||
brakeOutput(NULL);
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_APPLYING_BRAKE:
|
||||
if (!brake_data.brake_apply_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 1; // 刹车位置：1表示刹车
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_RELEASING_BRAKE:
|
||||
if (!brake_data.brake_release_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 0; // 刹车位置：0表示未刹车
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("ERROR: Unknown state\n");
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
// 如果刹车位置有变化，存入EEPROM
|
||||
if (brake_data.brake_position != brake_data.old_brake_position)
|
||||
{
|
||||
setParam("brk_pos", (float)brake_data.brake_position);
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
|
||||
|
||||
|
||||
printf("writeE2 brake_position = %d\n",brake_data.brake_position);
|
||||
|
||||
}
|
||||
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
|
||||
}
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void brakeInput(void *signal_id)
|
||||
{
|
||||
// BrakeSystem old_data = brake_data; // 定义并初始化old_data
|
||||
|
||||
// 填充数据
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
brake_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) )// 遥控器断线，不更新数据
|
||||
{
|
||||
brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
|
||||
brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
|
||||
}
|
||||
else if (signal_id == &can_fault_info)
|
||||
{
|
||||
brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
|
||||
brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
|
||||
}
|
||||
else if (signal_id == ðernet_fault_Info)
|
||||
{
|
||||
brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void paramUpdate(void *signal_id)
|
||||
{
|
||||
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void brakeAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
memset(&brake_data, 0, sizeof(BrakeSystem));
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
// 初始化时恢复刹车位置
|
||||
brake_data.brake_position = (uint8_t)getParam("brk_pos");
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
// 根据刹车位置恢复刹车状态
|
||||
if (brake_data.brake_position == 1)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
}
|
||||
|
||||
// 初始化定时器
|
||||
timerInit(&brake_data.brake_timer);
|
||||
timerInit(&brake_data.brake_apply_timer);
|
||||
timerInit(&brake_data.brake_release_timer);
|
||||
timerInit(&brake_data.brake_param_timer);
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
subscribe(&un_sw_sample, brakeInput);
|
||||
subscribe(&un_remote_control_input, brakeInput);
|
||||
subscribe(&can_fault_info, brakeInput);
|
||||
subscribe(ðernet_fault_Info, brakeInput);
|
||||
|
||||
|
||||
// 订阅定时器信号,用于状态机的定时处理
|
||||
subscribe(&brake_data.brake_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_param_timer, paramUpdate);
|
||||
|
||||
// 启动定时器，定期调用 brakeTimerProcess
|
||||
timerStart(&brake_data.brake_timer, 500, 1);
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
|
||||
printf("app_brake: 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"
|
||||
|
||||
#include "app_brake.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setBrakeOn(void) { return 1; }
|
||||
static inline uint8_t setBrakeOff(void) { return 0; }
|
||||
|
||||
BrakeSystem brake_data;
|
||||
|
||||
// 判断是否需要刹车
|
||||
static uint8_t shouldApplyBrake()
|
||||
{
|
||||
return (brake_data.emergency_stop_switch ||
|
||||
brake_data.remote_emergency_stop ||
|
||||
(brake_data.mode_signal == 0 && brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
|
||||
}
|
||||
|
||||
// 判断是否需要释放刹车
|
||||
static uint8_t shouldReleaseBrake()
|
||||
{
|
||||
return (!brake_data.emergency_stop_switch &&
|
||||
!brake_data.remote_emergency_stop &&
|
||||
((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 输出处理函数
|
||||
static void brakeOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// 根据电机状态，填充发送数据结构，发送信号
|
||||
switch (brake_data.brake_motor_state)
|
||||
{
|
||||
case 1: // 电机前进状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
printf("Brake: Motor forward\n");
|
||||
break;
|
||||
|
||||
case 2: // 电机后退状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOn();
|
||||
printf("Brake: Motor reverse\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBrakeOff();
|
||||
printf("Brake: Motor off\n");
|
||||
break;
|
||||
}
|
||||
|
||||
publishMessage(&un_h_bridge_output, 1);
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改刹车定时器处理函数
|
||||
static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
//#ifdef OIL_BRAKE
|
||||
switch (brake_data.state)
|
||||
{
|
||||
case BRAKE_STATE_IDLE:
|
||||
if (shouldApplyBrake())
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_BRAKE_ON:
|
||||
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
|
||||
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_APPLYING_BRAKE:
|
||||
if (!brake_data.brake_apply_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 1; // 刹车位置：1表示刹车
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_RELEASING_BRAKE:
|
||||
if (!brake_data.brake_release_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 0; // 刹车位置：0表示未刹车
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("ERROR: Unknown state\n");
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
// 如果刹车位置有变化，存入EEPROM
|
||||
if (brake_data.brake_position != brake_data.old_brake_position)
|
||||
{
|
||||
setParam("brk_pos", (float)brake_data.brake_position);
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
|
||||
printf("writeE2 brake_position = %d\n",brake_data.brake_position);
|
||||
|
||||
}
|
||||
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
|
||||
}
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void brakeInput(void *signal_id)
|
||||
{
|
||||
// BrakeSystem old_data = brake_data; // 定义并初始化old_data
|
||||
|
||||
// 填充数据
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
brake_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) )// 遥控器断线，不更新数据
|
||||
{
|
||||
brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
|
||||
brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
|
||||
}
|
||||
else if (signal_id == &can_fault_info)
|
||||
{
|
||||
brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
|
||||
brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
|
||||
}
|
||||
else if (signal_id == ðernet_fault_Info)
|
||||
{
|
||||
brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void paramUpdate(void *signal_id)
|
||||
{
|
||||
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void brakeAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
memset(&brake_data, 0, sizeof(BrakeSystem));
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
// 初始化时恢复刹车位置
|
||||
brake_data.brake_position = (uint8_t)getParam("brk_pos");
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
// 根据刹车位置恢复刹车状态
|
||||
if (brake_data.brake_position == 1)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
}
|
||||
|
||||
// 初始化定时器
|
||||
timerInit(&brake_data.brake_timer);
|
||||
timerInit(&brake_data.brake_apply_timer);
|
||||
timerInit(&brake_data.brake_release_timer);
|
||||
timerInit(&brake_data.brake_param_timer);
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
subscribe(&un_sw_sample, brakeInput);
|
||||
subscribe(&un_remote_control_input, brakeInput);
|
||||
subscribe(&can_fault_info, brakeInput);
|
||||
subscribe(ðernet_fault_Info, brakeInput);
|
||||
|
||||
|
||||
// 订阅定时器信号,用于状态机的定时处理
|
||||
subscribe(&brake_data.brake_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_param_timer, paramUpdate);
|
||||
|
||||
// 启动定时器，定期调用 brakeTimerProcess
|
||||
timerStart(&brake_data.brake_timer, 500, 1);
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
|
||||
printf("app_brake: initial OK \n");
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user