373 lines
12 KiB
C
373 lines
12 KiB
C
#include "app_config.h"
|
||
#include "app_dependence.h"
|
||
#include "interface.h"
|
||
|
||
#include "app_turn.h"
|
||
#include "app_differential_drive.h"
|
||
#include "app_param_manage.h"
|
||
#include <math.h>
|
||
#include <stdio.h>
|
||
|
||
TurnData turn_data;
|
||
Timer turn_timer2; // 定时器结构体
|
||
|
||
|
||
// 浮点数转uint32_t（按小端序存储）
|
||
void floatToUint32(float num, uint32_t *result)
|
||
{
|
||
// 将浮点数的内存数据直接拷贝到uint32_t变量
|
||
memcpy(result, &num, sizeof(num));
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
static void turnOutput(void *signal_id)
|
||
{
|
||
(void)signal_id;
|
||
uint32_t float_temp = 0;
|
||
|
||
switch(turn_data.direction)
|
||
{
|
||
case 0:
|
||
//左电机
|
||
floatToUint32(turn_data.left_zero_position,&float_temp);
|
||
un_sdo_output1.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.left_speed*2,&float_temp);
|
||
un_sdo_output2.bit_data.data = float_temp;
|
||
//右电机
|
||
floatToUint32(turn_data.right_zero_position,&float_temp);
|
||
un_sdo_output5.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.right_speed*2,&float_temp);
|
||
un_sdo_output6.bit_data.data = float_temp;
|
||
break;
|
||
case 1://左转
|
||
//左电机
|
||
floatToUint32(turn_data.inner_rad + turn_data.left_zero_position,&float_temp);
|
||
un_sdo_output1.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.left_speed,&float_temp);
|
||
un_sdo_output2.bit_data.data = float_temp;
|
||
//右电机
|
||
floatToUint32(turn_data.outer_rad + turn_data.right_zero_position,&float_temp);
|
||
un_sdo_output5.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.right_speed,&float_temp);
|
||
un_sdo_output6.bit_data.data = float_temp;
|
||
break;
|
||
case 2://右转
|
||
//左电机
|
||
floatToUint32(turn_data.outer_rad + turn_data.left_zero_position,&float_temp);
|
||
un_sdo_output1.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.left_speed,&float_temp);
|
||
un_sdo_output2.bit_data.data = float_temp;
|
||
//右电机
|
||
floatToUint32(turn_data.inner_rad + turn_data.right_zero_position,&float_temp);
|
||
un_sdo_output5.bit_data.data = float_temp;
|
||
floatToUint32(turn_data.right_speed,&float_temp);
|
||
un_sdo_output6.bit_data.data = float_temp;
|
||
break;default:;
|
||
}
|
||
|
||
un_sdo_output1.bit_data.index = LOC_REF_INDEX;
|
||
un_sdo_output1.bit_data.object_index = 0x0000;
|
||
// publishMessage(&un_sdo_output1, 1);//设定左电机位置
|
||
|
||
un_sdo_output2.bit_data.index = LIMIT_SPEED_INDEX;
|
||
un_sdo_output2.bit_data.object_index = 0x0000;
|
||
// publishMessage(&un_sdo_output2, 1);//设定右电机位置
|
||
|
||
un_sdo_output5.bit_data.index = LOC_REF_INDEX;
|
||
un_sdo_output5.bit_data.object_index = 0x0000;
|
||
// publishMessage(&un_sdo_output5, 1);//设定左电机位置
|
||
|
||
un_sdo_output6.bit_data.index = LIMIT_SPEED_INDEX;
|
||
un_sdo_output6.bit_data.object_index = 0x0000;
|
||
// publishMessage(&un_sdo_output6, 1);//设定右电机位置
|
||
|
||
}
|
||
|
||
//// 预充完成 需要先发一个失能，再使能
|
||
//void turnChargeFinish(uint8_t *stt, uint8_t *cnt)
|
||
//{
|
||
// switch(stt[0])//先发送切换模式以及电机失能，后面直接使能
|
||
// {
|
||
// case 0:
|
||
// if(cnt[0] >= 20)//发送50次
|
||
// {
|
||
// cnt[0] = 0;
|
||
// stt[0] = 1;
|
||
// }
|
||
// else
|
||
// {
|
||
// un_sdo_output5.bit_data.cmd = 0x2F;
|
||
// un_sdo_output5.bit_data.object_index = 0x6060;
|
||
// un_sdo_output5.bit_data.sub_index = 0;
|
||
// un_sdo_output5.bit_data.data = 0x01;//设定位置模式
|
||
// publishMessage(&un_sdo_output5, 1);
|
||
//
|
||
// un_sdo_output4.bit_data.cmd = 0x2B;
|
||
// un_sdo_output4.bit_data.object_index = 0x6040;
|
||
// un_sdo_output4.bit_data.sub_index = 0;
|
||
// un_sdo_output4.bit_data.data = 0x06;//电机失能
|
||
// publishMessage(&un_sdo_output4, 1);
|
||
// cnt[0] ++;
|
||
// stt[0] = 0;
|
||
// printf("position! \n");
|
||
// }
|
||
// break;
|
||
//
|
||
// case 1:
|
||
//
|
||
// cnt[0] = 0;
|
||
// stt[0] = 1;
|
||
// printf("disable! \n");
|
||
// break;default:;
|
||
// }
|
||
//}
|
||
|
||
/**
|
||
* 计算阿克曼转向几何中的外轮转向角（弧度制）
|
||
* @param L 轴距（前后轮距离，单位：米）
|
||
* @param W 轮距（左右轮距离，单位：米）
|
||
* @param inner_rad 内轮转向角（弧度）
|
||
* @param[out] curvature 返回路径曲率（单位：1/米）
|
||
* @return 外轮转向角（弧度）
|
||
*/
|
||
float calculate_outer_angle(float L, float W, float inner_rad, float* curvature)
|
||
{
|
||
// 保存原始符号
|
||
int sign = (inner_rad < 0) ? -1 : 1;
|
||
|
||
// 使用绝对值进行计算
|
||
float inner_temp = fabsf(inner_rad);
|
||
|
||
// 修正转弯半径计算（关键修改点）
|
||
float R_inner = L / tanf(inner_temp); // 内轮转弯半径
|
||
float R_outer = R_inner + W; // 外轮转弯半径 = 内轮半径 + 轮距
|
||
|
||
float R = R_inner + W / 2; // 整车转弯半径
|
||
|
||
// 计算曲率（注意符号）
|
||
*curvature = sign / R;
|
||
|
||
// 计算外轮转向角：atan(L/R_outer)
|
||
float delta_o = atanf(L / R_outer);
|
||
|
||
// 恢复原始符号
|
||
return sign * delta_o;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
// 转向函数
|
||
static void turnProcess(void *signal_id)
|
||
{
|
||
static uint8_t turn_process_state = 0;
|
||
static uint8_t turn_process_cnt = 0;
|
||
|
||
(void)signal_id;
|
||
if((turn_data.desired_curvature) > 0.01)//右转
|
||
{
|
||
turn_data.direction = 1;
|
||
}
|
||
else if((turn_data.desired_curvature) < -0.01)//左转
|
||
{
|
||
turn_data.direction = 2;
|
||
}
|
||
else//回中
|
||
{
|
||
turn_data.direction = 0;
|
||
}
|
||
|
||
turn_data.inner_rad = turn_data.desired_curvature*turn_data.left_max_position/2.0;//计算角度，内轮角度 -2-2的曲率对应 转换为限定角度
|
||
turn_data.outer_rad = calculate_outer_angle(turn_data.wheel_base, turn_data.track_width, turn_data.inner_rad, &turn_data.curvature);//计算外轮角度以及整车曲率
|
||
|
||
|
||
|
||
|
||
|
||
// //解决上电未接can用上位机调试电机，抱闸未打开的问题。
|
||
// if(power_data.current_state == POWER_WORKING)//抱闸控制
|
||
// {
|
||
// un_inf_can_kgf_output1.bit_data.KGF01 = 1;//打开抱闸
|
||
// }
|
||
// else
|
||
// {
|
||
// un_inf_can_kgf_output1.bit_data.KGF01 = 0;//抱闸
|
||
// }
|
||
|
||
if ((turn_data.current_state == POWER_WORKING) && (1 == turn_data.motor_sleep))//work且电机上电才运行
|
||
{
|
||
switch(turn_process_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据
|
||
{
|
||
case 0:
|
||
if(turn_process_cnt >= 20)//发送50次
|
||
{
|
||
turn_process_cnt = 0;
|
||
turn_process_state = 1;
|
||
}
|
||
else
|
||
{
|
||
turn_process_cnt ++;
|
||
|
||
un_sdo_output4.bit_data.index = RUM_MODE;
|
||
un_sdo_output4.bit_data.object_index = 0x0;
|
||
un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//设定位置模式
|
||
publishMessage(&un_sdo_output4, 1);
|
||
|
||
un_sdo_output8.bit_data.index = RUM_MODE;
|
||
un_sdo_output8.bit_data.object_index = 0x0;
|
||
un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//设定位置模式
|
||
publishMessage(&un_sdo_output8, 1);
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
if(turn_process_cnt >= 10)//发送50次
|
||
{
|
||
turn_process_cnt = 0;
|
||
turn_process_state = 2;
|
||
}
|
||
else
|
||
{
|
||
turn_process_cnt ++;
|
||
|
||
un_sdo_output3.bit_data.index = 0x0;
|
||
un_sdo_output3.bit_data.object_index = 0x0;
|
||
un_sdo_output3.bit_data.data = 0x0;//
|
||
publishMessage(&un_sdo_output3, 1);
|
||
|
||
un_sdo_output7.bit_data.index = 0x0;
|
||
un_sdo_output7.bit_data.object_index = 0x0;
|
||
un_sdo_output7.bit_data.data = 0x0;//
|
||
publishMessage(&un_sdo_output7, 1);
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
turnOutput(signal_id);
|
||
turn_process_cnt = 0;
|
||
turn_process_state = 2;
|
||
break;default:;
|
||
}
|
||
}
|
||
else//每次掉电都得重新发送使能
|
||
{
|
||
turn_process_state = 0;
|
||
turn_process_cnt = 0;
|
||
turn_data.motor_sleep = 0;
|
||
}
|
||
}
|
||
|
||
// 处理所有输入信号的函数
|
||
static void turnInput(void *signal_id)
|
||
{
|
||
(void)signal_id;
|
||
|
||
if (signal_id == &diff_data)
|
||
{
|
||
turn_data.mode = diff_data.mode;
|
||
turn_data.desired_curvature = diff_data.desired_curvature;
|
||
}
|
||
else if(signal_id == &power_data)//电机上电
|
||
{
|
||
turn_data.current_state = power_data.current_state;
|
||
}
|
||
else{}
|
||
turn_data.motor_sleep = 1;
|
||
turnProcess(signal_id);//处理映射
|
||
}
|
||
|
||
|
||
void turnParametersInit(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
//读取参数
|
||
turn_data.right_max_position = (float)getParam("R_maxP"); // 右转最大位置，rad
|
||
turn_data.right_zero_position = (float)getParam("R_zeroP"); // 右转最大位置，rad
|
||
turn_data.right_speed = (float)getParam("R_speed"); // 右转最大位置，rad
|
||
|
||
turn_data.left_max_position = (float)getParam("L_maxP"); // 右转最大位置，rad
|
||
turn_data.left_zero_position = (float)getParam("L_zeroP"); // 右转最大位置，rad
|
||
turn_data.left_speed = (float)getParam("L_speed"); // 右转最大位置，rad
|
||
|
||
|
||
turn_data.wheel_base = (float)getParam("whl_bas"); //轴距
|
||
turn_data.track_width = (float)getParam("tra_wid"); //轮距
|
||
|
||
timerStart(&turn_data.turn_timer,1000,1);//1s调用一次
|
||
}
|
||
|
||
void motorInit(void *signal_id)
|
||
{
|
||
if(turn_data.current_state == POWER_WORKING)
|
||
{
|
||
un_sdo_output4.bit_data.index = RUM_MODE;
|
||
un_sdo_output4.bit_data.object_index = 0x0;
|
||
un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//设定位置模式
|
||
publishMessage(&un_sdo_output4, 1);
|
||
|
||
un_sdo_output8.bit_data.index = RUM_MODE;
|
||
un_sdo_output8.bit_data.object_index = 0x0;
|
||
un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//设定位置模式
|
||
publishMessage(&un_sdo_output8, 1);
|
||
|
||
un_sdo_output3.bit_data.index = 0x0;
|
||
un_sdo_output3.bit_data.object_index = 0x0;
|
||
un_sdo_output3.bit_data.data = 0x0;//
|
||
publishMessage(&un_sdo_output3, 1);
|
||
|
||
un_sdo_output7.bit_data.index = 0x0;
|
||
un_sdo_output7.bit_data.object_index = 0x0;
|
||
un_sdo_output7.bit_data.data = 0x0;//
|
||
publishMessage(&un_sdo_output7, 1);
|
||
}
|
||
// timerStart(&turn_timer2,1000,1);//1s调用一次
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
// 修改APP模块的初始化函数
|
||
void turnAppInit(void)
|
||
{
|
||
// 初始化
|
||
memset(&turn_data, 0, sizeof(TurnData));
|
||
timerInit(&turn_data.turn_timer);
|
||
timerInit(&turn_timer2);
|
||
|
||
|
||
//读取参数
|
||
turn_data.right_max_position = (float)getParam("R_maxP"); // 右转最大位置，rad
|
||
// turn_data.right_min_position = (float)getParam("R_minP"); // 右转最大位置，rad
|
||
turn_data.right_zero_position = (float)getParam("R_zeroP"); // 右转最大位置，rad
|
||
turn_data.right_speed = (float)getParam("R_speed"); // 右转最大位置，rad
|
||
|
||
turn_data.left_max_position = (float)getParam("L_maxP"); // 右转最大位置，rad
|
||
// turn_data.left_min_position = (float)getParam("L_minP"); // 右转最大位置，rad
|
||
turn_data.left_zero_position = (float)getParam("L_zeroP"); // 右转最大位置，rad
|
||
turn_data.left_speed = (float)getParam("L_speed"); // 右转最大位置，rad
|
||
|
||
// 订阅输入信号,处理刹车逻辑
|
||
subscribe(&diff_data, turnInput);
|
||
subscribe(&power_data, turnInput);
|
||
|
||
subscribe(&turn_data.turn_timer, turnParametersInit);
|
||
|
||
subscribe(&turn_timer2, motorInit);
|
||
|
||
|
||
timerStart(&turn_data.turn_timer,1000,1);//1s调用一次
|
||
timerStart(&turn_timer2,1000,1);//1s调用一次
|
||
|
||
printf("turnApp: initial OK \n");
|
||
}
|
||
|
||
|