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

第一次提交

Browse Source
This commit is contained in:
liumin
2025-08-26 10:53:01 +08:00
commit 7730610c15
59 changed files with 12914 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

372
app/app_turn.c Normal file
View File

@@ -0,0 +1,372 @@
#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; // <20><>ʱ<EFBFBD><CAB1><EFBFBD>ṹ<EFBFBD><E1B9B9>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תuint32_t<5F><74><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2>
void floatToUint32(float num, uint32_t *result)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD>uint32_t<5F><74><EFBFBD><EFBFBD>
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:
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
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://<2F><>ת
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
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://<2F><>ת
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
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);//<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
un_sdo_output2.bit_data.index = LIMIT_SPEED_INDEX;
un_sdo_output2.bit_data.object_index = 0x0000;
// publishMessage(&un_sdo_output2, 1);//<2F>趨<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB>
un_sdo_output5.bit_data.index = LOC_REF_INDEX;
un_sdo_output5.bit_data.object_index = 0x0000;
// publishMessage(&un_sdo_output5, 1);//<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
un_sdo_output6.bit_data.index = LIMIT_SPEED_INDEX;
un_sdo_output6.bit_data.object_index = 0x0000;
// publishMessage(&un_sdo_output6, 1);//<2F>趨<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB>
}
//// Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>Ҫ<EFBFBD>ȷ<EFBFBD>һ<EFBFBD><D2BB>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ʹ<EFBFBD><CAB9>
//void turnChargeFinish(uint8_t *stt, uint8_t *cnt)
//{
// switch(stt[0])//<2F>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>л<EFBFBD>ģʽ<C4A3>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ʹ<EFBFBD><CAB9>
// {
// case 0:
// if(cnt[0] >= 20)//<2F><><EFBFBD><EFBFBD>50<35><30>
// {
// 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;//<2F>趨λ<E8B6A8><CEBB>ģʽ
// 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;//<2F><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
// 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:;
// }
//}
/**
* <20><><EFBFBD>㰢<EFBFBD><E3B0A2><EFBFBD><EFBFBD>ת<EFBFBD>򼸺<EFBFBD><F2BCB8BA>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>
* @param L <20><><EFBFBD>ࣨǰ<E0A3A8><C7B0><EFBFBD>־<EFBFBD><D6BE>룬<EFBFBD><EBA3AC>λ<EFBFBD><CEBB><EFBFBD>ף<EFBFBD>
* @param W <20>־ࣨ<D6BE><E0A3A8><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>룬<EFBFBD><EBA3AC>λ<EFBFBD><CEBB><EFBFBD>ף<EFBFBD>
* @param inner_rad <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD>ȣ<EFBFBD>
* @param[out] curvature <20><><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>λ<EFBFBD><CEBB>1/<2F>ף<EFBFBD>
* @return <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD>ȣ<EFBFBD>
*/
float calculate_outer_angle(float L, float W, float inner_rad, float* curvature)
{
// <20><><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
int sign = (inner_rad < 0) ? -1 : 1;
// ʹ<>þ<EFBFBD><C3BE><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
float inner_temp = fabsf(inner_rad);
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶<EFBFBD><EBBEB6><EFBFBD>㣨<EFBFBD>ؼ<EFBFBD><D8BC>޸ĵ㣩
float R_inner = L / tanf(inner_temp); // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶
float R_outer = R_inner + W; // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶 = <20><><EFBFBD>ְ뾶 + <20>־<EFBFBD>
float R = R_inner + W / 2; // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD>ע<EFBFBD><D7A2><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>
*curvature = sign / R;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD>atan(L/R_outer)
float delta_o = atanf(L / R_outer);
// <20>ָ<EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
return sign * delta_o;
}
// ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)//<2F><>ת
{
turn_data.direction = 1;
}
else if((turn_data.desired_curvature) < -0.01)//<2F><>ת
{
turn_data.direction = 2;
}
else//<2F><><EFBFBD><EFBFBD>
{
turn_data.direction = 0;
}
turn_data.inner_rad = turn_data.desired_curvature*turn_data.left_max_position/2.0;//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD>ֽǶ<D6BD> -2-2<><32><EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD>Ӧ ת<><D7AA>Ϊ<EFBFBD>޶<EFBFBD><DEB6>Ƕ<EFBFBD>
turn_data.outer_rad = calculate_outer_angle(turn_data.wheel_base, turn_data.track_width, turn_data.inner_rad, &turn_data.curvature);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽǶ<D6BD><C7B6>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// //<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>δ<EFBFBD><CEB4>can<61><6E><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>բδ<D5A2>򿪵<EFBFBD><F2BFAAB5><EFBFBD><EFBFBD>⡣
// if(power_data.current_state == POWER_WORKING)//<2F><>բ<EFBFBD><D5A2><EFBFBD><EFBFBD>
// {
// un_inf_can_kgf_output1.bit_data.KGF01 = 1;//<2F>򿪱<EFBFBD>բ
// }
// else
// {
// un_inf_can_kgf_output1.bit_data.KGF01 = 0;//<2F><>բ
// }
if ((turn_data.current_state == POWER_WORKING) && (1 == turn_data.motor_sleep))//work<72>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
switch(turn_process_state)//<2F>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>л<EFBFBD>ģʽ<C4A3>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ʹ<EFBFBD><CAB9> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
case 0:
if(turn_process_cnt >= 20)//<2F><><EFBFBD><EFBFBD>50<35><30>
{
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;//<2F>趨λ<E8B6A8><CEBB>ģʽ
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;//<2F>趨λ<E8B6A8><CEBB>ģʽ
publishMessage(&un_sdo_output8, 1);
}
break;
case 1:
if(turn_process_cnt >= 10)//<2F><><EFBFBD><EFBFBD>50<35><30>
{
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//ÿ<>ε<EFBFBD><CEB5>綼<EFBFBD><E7B6BC><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD>ʹ<EFBFBD><CAB9>
{
turn_process_state = 0;
turn_process_cnt = 0;
turn_data.motor_sleep = 0;
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŵĺ<C5B5><C4BA><EFBFBD>
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)//<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>
{
turn_data.current_state = power_data.current_state;
}
else{}
turn_data.motor_sleep = 1;
turnProcess(signal_id);//<2F><><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3>
}
void turnParametersInit(void *signal_id)
{
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
turn_data.right_max_position = (float)getParam("R_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.right_zero_position = (float)getParam("R_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.right_speed = (float)getParam("R_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_max_position = (float)getParam("L_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_zero_position = (float)getParam("L_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_speed = (float)getParam("L_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.wheel_base = (float)getParam("whl_bas"); //<2F><><EFBFBD><EFBFBD>
turn_data.track_width = (float)getParam("tra_wid"); //<2F>־<EFBFBD>
timerStart(&turn_data.turn_timer,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
}
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;//<2F>趨λ<E8B6A8><CEBB>ģʽ
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;//<2F>趨λ<E8B6A8><CEBB>ģʽ
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<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
}
// <20>޸<EFBFBD>APPģ<50><C4A3><EFBFBD>ij<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void turnAppInit(void)
{
// <20><>ʼ<EFBFBD><CABC>
memset(&turn_data, 0, sizeof(TurnData));
timerInit(&turn_data.turn_timer);
timerInit(&turn_timer2);
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
turn_data.right_max_position = (float)getParam("R_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
// turn_data.right_min_position = (float)getParam("R_minP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.right_zero_position = (float)getParam("R_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.right_speed = (float)getParam("R_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_max_position = (float)getParam("L_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
// turn_data.left_min_position = (float)getParam("L_minP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_zero_position = (float)getParam("L_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
turn_data.left_speed = (float)getParam("L_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>,<2C><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD>߼<EFBFBD>
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<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
timerStart(&turn_timer2,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
printf("turnApp: initial OK \n");
}