遥控器类型在cmd.h里面修改
This commit is contained in:
parent
cfe106fa85
commit
8c5f3de185
@ -32,7 +32,6 @@
|
||||
/* USER CODE BEGIN Includes */
|
||||
#include "bsp_delay.h"
|
||||
#include "bsp_can.h"
|
||||
#include "Action.h"
|
||||
//#include "bsp_dwt.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
|
||||
@ -1144,30 +1144,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\action_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>action_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\navi_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>navi_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\r12ds_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>r12ds_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1183,7 +1159,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1195,7 +1171,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1207,7 +1183,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1219,7 +1195,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1231,7 +1207,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1243,7 +1219,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1253,18 +1229,6 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\Algorithm\navi.c</PathWithFileName>
|
||||
<FilenameWithoutPath>navi.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
@ -1275,7 +1239,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1287,7 +1251,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1299,7 +1263,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1311,7 +1275,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1323,7 +1287,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1335,7 +1299,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1347,19 +1311,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\device\Action.c</PathWithFileName>
|
||||
<FilenameWithoutPath>Action.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1379,7 +1331,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1391,7 +1343,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1403,7 +1355,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1423,7 +1375,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1443,7 +1395,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>93</FileNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1455,7 +1407,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>94</FileNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1467,7 +1419,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>95</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1479,7 +1431,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>96</FileNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -1228,16 +1228,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\dr16_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>action_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\action_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>navi_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\navi_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>r12ds_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
@ -1278,11 +1268,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Algorithm\pid.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>navi.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Algorithm\navi.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
@ -1318,11 +1303,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\device\dr16.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>Action.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\device\Action.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>r12ds.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
|
||||
Binary file not shown.
@ -1,216 +0,0 @@
|
||||
#include "navi.h"
|
||||
|
||||
|
||||
//导航初始化一系列参数
|
||||
int8_t Action_init(ops_t *o , const ops_param_t *param , Action_POS_t *pos)
|
||||
{
|
||||
if(o ==NULL) return DEVICE_ERR;
|
||||
if(param == NULL) return DEVICE_ERR;
|
||||
// while(!Action_ready)
|
||||
// {
|
||||
// TIM4->CCR3 = 9999;//等待全场定位初始化
|
||||
// }
|
||||
|
||||
// TIM4->CCR3 = 0;
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
|
||||
//
|
||||
|
||||
// osDelay(5);
|
||||
|
||||
o->param = param;
|
||||
/*全场定位pid初始化*/
|
||||
//全场定位跑路径的xy方向速度环pid
|
||||
PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
|
||||
PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
|
||||
/* 初始化角度纠正PID */
|
||||
PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
|
||||
PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
|
||||
//底盘xy方向位置环pid初始化
|
||||
//内环
|
||||
PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
|
||||
PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
|
||||
//外环
|
||||
PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
|
||||
PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));
|
||||
|
||||
|
||||
|
||||
//底盘路径速度pid初始化
|
||||
PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
|
||||
//全场定位底盘坐标初始化
|
||||
o->chassis_pos->pos_x=0;
|
||||
o->chassis_pos->pos_y=0;
|
||||
o->chassis_pos->pos_lastX=0;
|
||||
o->chassis_pos->pos_lastY=0;
|
||||
o->chassis_map =param->path ;
|
||||
o->chassis_pos = pos;
|
||||
|
||||
|
||||
//设置全场定位允许的误差范围
|
||||
o->state.mistake = POS_ALLOW_MISTAKE;
|
||||
o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
|
||||
o->state.moveState = START;
|
||||
o->state.points_num = param->path_num;
|
||||
//标志位初始化
|
||||
o->POS_IS_CPT = NO;
|
||||
|
||||
|
||||
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
//利用C板imu纠正角度
|
||||
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
|
||||
{
|
||||
fp32 delta_angle,delta_w;
|
||||
//外环角度纠正
|
||||
delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
|
||||
//内环速度纠正
|
||||
delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
|
||||
return delta_w;
|
||||
}
|
||||
|
||||
//底盘xy速度计算(全场定位)
|
||||
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
|
||||
|
||||
|
||||
o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
|
||||
o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
|
||||
o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
//定点控制
|
||||
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
|
||||
{
|
||||
fp32 delta_x,delta_y;
|
||||
fp32 delta_vx,delta_vy;
|
||||
fp32 vx_set,vy_set;//x,y,w目标速度
|
||||
|
||||
//x
|
||||
delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
|
||||
delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
|
||||
|
||||
//y
|
||||
delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
|
||||
delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
|
||||
|
||||
|
||||
|
||||
|
||||
vx_set = delta_vx;
|
||||
vy_set = delta_vy;
|
||||
|
||||
POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//判断是否到达函数(全场定位)，最最后判断
|
||||
bool isArrive(point_t p,ops_t *o)
|
||||
{
|
||||
uint16_t xArrive = 0, yArrive = 0, zArrive=0;
|
||||
|
||||
xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
|
||||
yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
|
||||
|
||||
if(xArrive && yArrive) return true;
|
||||
else return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//寻迹，跑点函数
|
||||
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
|
||||
{
|
||||
static fp32 distance;//两点之间的距离差
|
||||
static fp32 chassis_speed_set;//底盘速度设置
|
||||
static fp32 SIN,COS;//方向解算
|
||||
static fp32 world_vx,world_vy;//x,y方向分解速度
|
||||
static fp32 chassis_vx,chassis_vy;//底盘xy分速度
|
||||
static int cnt =0 ;//计数标志位
|
||||
|
||||
static int flag_update_num;//记录flag更新，防止cnt一直变化
|
||||
|
||||
/*数据同步*/
|
||||
o->current_x =o->chassis_pos ->pos_x ;
|
||||
o->current_y =o->chassis_pos ->pos_y ;
|
||||
o->next_mapx = o->chassis_map[cnt].x;
|
||||
o->next_mapy =o->chassis_map[cnt].y;
|
||||
|
||||
|
||||
o->Navi_Mode .Trig_flag = ops_out ->flag ;
|
||||
|
||||
if(o == NULL) return DEVICE_ERR;
|
||||
if (ops_out == NULL) return DEVICE_ERR;
|
||||
|
||||
// static fp32 yaw_angle_set = 0;
|
||||
if(o->POS_IS_CPT == NO)//路径未跑完
|
||||
{
|
||||
if(o->state.moveState ==START)
|
||||
{
|
||||
//更新路径状态
|
||||
o->state.moveState =MOVING;
|
||||
}
|
||||
|
||||
|
||||
distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
|
||||
+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
|
||||
|
||||
chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
|
||||
//速度分解
|
||||
SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
|
||||
COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
|
||||
|
||||
world_vx = chassis_speed_set * COS;
|
||||
world_vy = chassis_speed_set * SIN;
|
||||
|
||||
// chassis_vx = world_vx;
|
||||
// chassis_vy = world_vy;
|
||||
//世界坐标转机器坐标系转换
|
||||
|
||||
chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
|
||||
chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
|
||||
|
||||
}
|
||||
|
||||
/*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/
|
||||
|
||||
|
||||
if(isArrive(o->chassis_map[cnt],o)) //判断是否到达(全场定位)，只涉及xy的判断
|
||||
{
|
||||
/*到达点后，这里应该增加cnt增加或减小的触发方式*/
|
||||
|
||||
if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt
|
||||
if (o->Navi_Mode .Trig_flag==1) cnt++; }
|
||||
else if (o->Navi_Mode .Trig_flag == -1) cnt--;
|
||||
flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
|
||||
}
|
||||
else
|
||||
{
|
||||
o->POS_IS_CPT = NO;
|
||||
hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle); //调用该函数使临近误差值快速响应
|
||||
}
|
||||
if(cnt>3)//根据点数更改
|
||||
{
|
||||
o->POS_IS_CPT =YES;
|
||||
//到达目的地
|
||||
o->final_out .vx =0;
|
||||
o->final_out .vy =0;
|
||||
o->final_out.yaw_angle=0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
ops_out->out.Vx= o->final_out .vx ;
|
||||
ops_out->out.Vy = o->final_out .vy ;
|
||||
ops_out->out.Vw = o->final_out.yaw_angle;
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
@ -1,108 +0,0 @@
|
||||
#ifndef NAVI_H
|
||||
#define NAVI_H
|
||||
|
||||
|
||||
#include "Action.h"
|
||||
#include "pid.h"
|
||||
#include "ahrs.h"
|
||||
#include "bmi088.h"
|
||||
#include "map.h"
|
||||
|
||||
typedef enum {
|
||||
AUTO_MODE, // 自动跑点模式
|
||||
MANUAL_MODE , // 控制模式
|
||||
NO_MODE
|
||||
} Navi_Mode_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
Navi_Mode_t mode;
|
||||
|
||||
int Trig_flag;//触发沿，初始值为零，两种控制模式下，这个值变为1，会使cnt+1，为-1，使cnt-1
|
||||
|
||||
} Navi_COT_MODE_t;
|
||||
|
||||
|
||||
typedef struct{
|
||||
float pos_x;
|
||||
float pos_y;
|
||||
float last_pos_x;
|
||||
float last_pos_y;
|
||||
}chassis_position_t;
|
||||
|
||||
typedef enum{
|
||||
YES,
|
||||
NO,
|
||||
NEXT1,
|
||||
NEXT2
|
||||
}is_cpt_e;
|
||||
typedef struct{
|
||||
pid_param_t pid_PosSpeed_x_param;
|
||||
pid_param_t pid_PosSpeed_y_param;
|
||||
pid_param_t pid_pos_x_out_param;
|
||||
pid_param_t pid_pos_x_inner_param;
|
||||
pid_param_t pid_pos_y_out_param;
|
||||
pid_param_t pid_pos_y_inner_param;
|
||||
pid_param_t pid_OutAngle_param;
|
||||
pid_param_t pid_InnerAngle_param;
|
||||
pid_param_t path_speed_pid_param;
|
||||
|
||||
}ops_pid_param_t;
|
||||
|
||||
typedef struct {
|
||||
ops_pid_param_t ops_pid;
|
||||
const point_t *path;
|
||||
int8_t path_num;
|
||||
}ops_param_t;
|
||||
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
*/
|
||||
typedef struct {
|
||||
Action_POS_t *chassis_pos;
|
||||
fp32 current_x;
|
||||
fp32 current_y;
|
||||
|
||||
fp32 next_mapx;
|
||||
fp32 next_mapy;
|
||||
const point_t *chassis_map;
|
||||
const ops_param_t *param;
|
||||
PathState_t state;
|
||||
Navi_COT_MODE_t Navi_Mode;
|
||||
|
||||
AHRS_Eulr_t ops_imu_pos;
|
||||
AHRS_Gyro_t ops_gyro;
|
||||
|
||||
|
||||
|
||||
pid_type_def pid_PosSpeed_x;
|
||||
pid_type_def pid_PosSpeed_y;
|
||||
pid_type_def pid_pos_x_out;
|
||||
pid_type_def pid_pos_x_inner;
|
||||
pid_type_def pid_pos_y_out;
|
||||
pid_type_def pid_pos_y_inner;
|
||||
pid_type_def pid_OutAngle;
|
||||
pid_type_def pid_InnerAngle;
|
||||
pid_type_def path_speed_pid;
|
||||
|
||||
|
||||
is_cpt_e POS_IS_CPT;
|
||||
|
||||
struct
|
||||
{
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 yaw_angle;
|
||||
}final_out;
|
||||
|
||||
}ops_t;
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
|
||||
|
||||
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
|
||||
#endif
|
||||
|
||||
@ -1,6 +1,5 @@
|
||||
#include "Chassis.h"
|
||||
#include "gpio.h"
|
||||
#include "Action.h"
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
@ -113,16 +112,11 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
|
||||
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
|
||||
{
|
||||
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
|
||||
c->hopemotorout.OmniSpeedOut[2] = -Vx-Vy+Vw;//右后
|
||||
c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后
|
||||
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
|
||||
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
|
||||
c->hopemotorout.OmniSpeedOut[0] = Vx+Vy+Vw;//左前
|
||||
}
|
||||
|
||||
// v[0] = (vx_set+vy_set+vw_set)*mul + gyro_z;//左前
|
||||
// v[1] = (-vx_set+vy_set+vw_set)*mul + gyro_z;//右前
|
||||
// v[2] = (-vx_set-vy_set+vw_set)*mul + gyro_z;//右后
|
||||
// v[3] = (vx_set-vy_set+vw_set)*mul + gyro_z;//左后
|
||||
|
||||
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
|
||||
//{
|
||||
// uint16_t xArrive = 0, yArrive = 0;
|
||||
|
||||
@ -31,7 +31,6 @@
|
||||
#include "can_use.h"
|
||||
#include "cmd.h"
|
||||
#include "filter.h"
|
||||
#include "Action.h"
|
||||
|
||||
#define CHASSIS_OK (0)
|
||||
#define CHASSIS_ERR (-1)
|
||||
@ -125,7 +124,6 @@ typedef struct{
|
||||
const Chassis_Param_t *param; //一些固定的参数
|
||||
|
||||
ChassisImu_t pos088; //088的实时姿态
|
||||
Action_POS_t Action_pos;
|
||||
CMD_Chassis_CtrlType_e ctrl;
|
||||
CMD_Chassis_mode_e mode;
|
||||
|
||||
|
||||
@ -193,93 +193,7 @@ static const ConfigParam_t param_chassis ={
|
||||
|
||||
},
|
||||
|
||||
|
||||
|
||||
/*码盘导航*/
|
||||
|
||||
.ops={
|
||||
|
||||
/*导航点*/
|
||||
.path=map_point,
|
||||
.path_num = point_num ,
|
||||
.ops_pid={
|
||||
/*路径速度*/
|
||||
.path_speed_pid_param=
|
||||
{
|
||||
.p = 15.1f,
|
||||
.i = 0.02f,
|
||||
.d = 3.2f,
|
||||
.i_limit = 200.0f,
|
||||
.out_limit =6000.0f,
|
||||
|
||||
|
||||
},
|
||||
.pid_PosSpeed_x_param={
|
||||
.p =1.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
.pid_PosSpeed_y_param={
|
||||
.p =1.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
.pid_pos_x_out_param={
|
||||
.p =1.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
.pid_pos_x_inner_param={
|
||||
.p =2.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
.pid_pos_y_out_param={
|
||||
.p =1.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
.pid_pos_y_inner_param={
|
||||
.p =2.0f,
|
||||
.i =0.001f,
|
||||
.d =0.0f,
|
||||
.i_limit =15.0f,
|
||||
.out_limit =4000.0f,
|
||||
},
|
||||
/*码盘导航陀螺仪角度纠正内外环*/
|
||||
.pid_OutAngle_param={
|
||||
.p = 5.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit =100.0f,
|
||||
},
|
||||
.pid_InnerAngle_param={
|
||||
.p = 100.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit =1000.0f,
|
||||
},
|
||||
|
||||
|
||||
|
||||
|
||||
},
|
||||
|
||||
|
||||
},
|
||||
|
||||
|
||||
.can = {
|
||||
.pitch6020 = BSP_CAN_1,
|
||||
.motor3508 = BSP_CAN_1,
|
||||
@ -293,11 +207,7 @@ const ConfigParam_t *Config_ChassisGet(void)
|
||||
return ¶m_chassis;
|
||||
}
|
||||
|
||||
///*获取导航地图*/
|
||||
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
|
||||
// config->ops.path = path;
|
||||
// config->ops.path_num = path_num;
|
||||
//}
|
||||
|
||||
/**
|
||||
* \brief 从Flash读取配置信息
|
||||
*
|
||||
|
||||
@ -4,13 +4,11 @@
|
||||
#include "Chassis.h"
|
||||
#include "can_use.h"
|
||||
#include "ahrs.h"
|
||||
#include "navi.h"
|
||||
#include "map.h"
|
||||
|
||||
|
||||
typedef struct{
|
||||
Chassis_Param_t chassis; /**/
|
||||
ops_param_t ops;
|
||||
CAN_Params_t can;
|
||||
AHRS_Eulr_t mech_zero[4];
|
||||
|
||||
|
||||
@ -1,53 +0,0 @@
|
||||
#include "bsp_action.h"
|
||||
#include "main.h"
|
||||
|
||||
|
||||
extern UART_HandleTypeDef huart1;
|
||||
extern DMA_HandleTypeDef hdma_usart1_rx;
|
||||
void RS232_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
||||
{
|
||||
|
||||
//enable the dma transfer for the receiver request
|
||||
SET_BIT(huart1.Instance->CR3, USART_CR3_DMAR);
|
||||
|
||||
//enable idle interrupt
|
||||
__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
|
||||
|
||||
//disable dma, to change the dma register
|
||||
__HAL_DMA_DISABLE(&hdma_usart1_rx);
|
||||
|
||||
//disable dma again but why?
|
||||
//what's the condition?
|
||||
while(hdma_usart1_rx.Instance->CR & DMA_SxCR_EN)
|
||||
{
|
||||
__HAL_DMA_DISABLE(&hdma_usart1_rx);
|
||||
}
|
||||
|
||||
//??
|
||||
hdma_usart1_rx.Instance->PAR = (uint32_t) & (USART1->DR);
|
||||
|
||||
//memory buffer 1
|
||||
hdma_usart1_rx.Instance->M0AR = (uint32_t)(rx1_buf);
|
||||
|
||||
//momory buffer 2
|
||||
hdma_usart1_rx.Instance->M1AR = (uint32_t)(rx2_buf);
|
||||
|
||||
//data length
|
||||
hdma_usart1_rx.Instance->NDTR = dma_buf_num;
|
||||
|
||||
//enable double memory buffer
|
||||
SET_BIT(hdma_usart1_rx.Instance->CR, DMA_SxCR_DBM);
|
||||
|
||||
|
||||
//enable dma
|
||||
__HAL_DMA_ENABLE(&hdma_usart1_rx);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,8 +0,0 @@
|
||||
#ifndef BSP_ACTION_H
|
||||
#define BSP_ACTION_H
|
||||
#include "struct_typedef.h"
|
||||
|
||||
void RS232_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,203 +0,0 @@
|
||||
|
||||
/**
|
||||
* 东北大学ACTION码盘驱动
|
||||
*
|
||||
* 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
|
||||
* 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
|
||||
*
|
||||
* 使用手册：
|
||||
* 请参阅相关文档了解如何使用本驱动。
|
||||
*
|
||||
* 注意：
|
||||
* 本驱动仅适用于东北大学ACTION码盘。
|
||||
|
||||
*删去Action_HandleOffline错误处理函数中对结构体清0的函数
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "tim.h"
|
||||
#include "Action.h"
|
||||
#include <string.h>
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
|
||||
uint8_t rxbuf[RS232_FRAME_LENGTH];
|
||||
|
||||
static void Ops10msTimerCallback(void *arg){
|
||||
(void)arg;
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
|
||||
}
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
static void ACTION_IdleCallback(void) {
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t ACTIONRECV_Init(Action_POS_t *pos){
|
||||
if(pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
pos->Action_ready =0;//码盘校准标志位初始化
|
||||
|
||||
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
|
||||
pos->action_timer_Id =
|
||||
osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
|
||||
|
||||
osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
|
||||
ACTION_IdleCallback);
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t ACTION_StartReceiving() {
|
||||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
|
||||
(uint8_t *)rxbuf,
|
||||
sizeof(rxbuf)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
bool_t ACTION_WaitDmaCplt(void) {
|
||||
return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
|
||||
SIGNAL_ACTION_RAW_REDY);
|
||||
}
|
||||
|
||||
|
||||
/* *
|
||||
|
||||
|
||||
*/
|
||||
int8_t ACTION_Parse(Action_POS_t *pos)
|
||||
{
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
static union
|
||||
{
|
||||
fp32 pos_data[3];
|
||||
char rxbuff[12];
|
||||
} pos_rxbuf;
|
||||
|
||||
|
||||
if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
|
||||
{
|
||||
if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
|
||||
{
|
||||
|
||||
pos_rxbuf.rxbuff[0]=rxbuf[2];
|
||||
pos_rxbuf.rxbuff[1]=rxbuf[3];
|
||||
pos_rxbuf.rxbuff[2]=rxbuf[4];
|
||||
pos_rxbuf.rxbuff[3]=rxbuf[5];
|
||||
|
||||
|
||||
pos_rxbuf.rxbuff[4]=rxbuf[14];
|
||||
pos_rxbuf.rxbuff[5]=rxbuf[15];
|
||||
pos_rxbuf.rxbuff[6]=rxbuf[16];
|
||||
pos_rxbuf.rxbuff[7]=rxbuf[17];
|
||||
|
||||
|
||||
pos_rxbuf.rxbuff[8]=rxbuf[18];
|
||||
pos_rxbuf.rxbuff[9]=rxbuf[19];
|
||||
pos_rxbuf.rxbuff[10]=rxbuf[20];
|
||||
pos_rxbuf.rxbuff[11]=rxbuf[21];
|
||||
|
||||
|
||||
|
||||
// 数据解析
|
||||
pos->pos_yaw = pos_rxbuf.pos_data[0];
|
||||
// 按照安装方向决定正负号
|
||||
pos->pos_x = pos_rxbuf.pos_data[1];
|
||||
pos->pos_y = pos_rxbuf.pos_data[2];
|
||||
|
||||
pos ->Action_ready =1;//码盘校准完成
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
//该函数用来计算速度度(利用获取的位置来计算相应的速度)
|
||||
//此处获取的是真实的位置(mm)后每10ms做一次微分处理
|
||||
int8_t ACTION_DataRefresh(Action_POS_t *pos){
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
|
||||
osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
|
||||
//计算每毫秒的速度 该函数每10ms调用
|
||||
pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
|
||||
pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
|
||||
|
||||
pos->pos_lastX = pos->pos_x;
|
||||
pos->pos_lastY = pos->pos_y;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int8_t Action_HandleOffline(Action_POS_t *pos) {
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
(void)pos;
|
||||
|
||||
// memset(pos, 0, sizeof(*pos));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* 将字符串拼接 */
|
||||
void Strcat(char str1[], char str2[], uint8_t num)
|
||||
{
|
||||
int i = 0, j = 0;
|
||||
|
||||
while (str1[i] != '\0')
|
||||
i++;
|
||||
|
||||
for (j = 0; j < num; j++)
|
||||
{
|
||||
str1[i++] = str2[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* 该部分函数用于码盘的重定位系统 */
|
||||
/* ----------------------------- */
|
||||
|
||||
/* 手动标定，用于校正 */
|
||||
void ACT_Calibration(void)
|
||||
{
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
|
||||
}
|
||||
|
||||
/* 清零 */
|
||||
/* 将当前位置设置为 (0, 0) */
|
||||
void ACT_ZeroClear(void)
|
||||
{
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
|
||||
}
|
||||
|
||||
/* 更新 XY 坐标 */
|
||||
/* 将当前位置更新为指定的 X 和 Y */
|
||||
void ACT_UpdateXY(float pos_x, float pos_y)
|
||||
{
|
||||
char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
|
||||
static union
|
||||
{
|
||||
float XY[2]; // 两个浮点数表示 X 和 Y 坐标
|
||||
char data[8]; // 将浮点数视为字节数据
|
||||
} set;
|
||||
|
||||
set.XY[0] = pos_x;
|
||||
set.XY[1] = pos_y;
|
||||
|
||||
Strcat(update_xy, set.data, 8);
|
||||
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,59 +0,0 @@
|
||||
#ifndef _ACTION_H_
|
||||
#define _ACTION_H_
|
||||
|
||||
#define RS232_FRAME_LENGTH 28u
|
||||
#define REF_LEN_RX_BUFF 0xFF
|
||||
|
||||
#include <cmsis_os2.h>
|
||||
#include "device.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "cmd.h"
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
osTimerId_t action_timer_Id;
|
||||
/*原始数据*/
|
||||
fp32 pos_x;
|
||||
fp32 pos_y;
|
||||
fp32 pos_yaw; // 航向角速度
|
||||
/*处理数据*/
|
||||
fp32 pos_Vx;
|
||||
fp32 pos_Vy;
|
||||
fp32 pos_lastX;
|
||||
fp32 pos_lastY;
|
||||
|
||||
int Action_ready;//码盘校准标志位
|
||||
|
||||
|
||||
}Action_POS_t;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t ACTIONRECV_Init(Action_POS_t*pos);
|
||||
|
||||
int8_t ACTION_StartReceiving(void);
|
||||
|
||||
bool_t ACTION_WaitDmaCplt(void);
|
||||
|
||||
int8_t ACTION_Parse(Action_POS_t*pos);
|
||||
|
||||
int8_t ACTION_DataRefresh(Action_POS_t*pos);
|
||||
|
||||
int8_t Action_HandleOffline(Action_POS_t *pos);
|
||||
|
||||
void ACT_UpdateXY(float pos_x,float pos_y);
|
||||
|
||||
void ACT_ZeroClear(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -13,6 +13,17 @@
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
/*Export function --------------------------------------------------------------*/
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef dr16_t
|
||||
|
||||
|
||||
#else
|
||||
|
||||
|
||||
#endif
|
||||
int8_t CMD_Init(CMD_t *cmd){
|
||||
/*若主结构体为空 自动返回错误 */
|
||||
if(cmd == NULL) return-1;
|
||||
@ -23,12 +34,14 @@ int8_t CMD_Init(CMD_t *cmd){
|
||||
return 0;
|
||||
}
|
||||
|
||||
float aaaa;
|
||||
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
||||
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
|
||||
|
||||
#ifdef r12ds_t
|
||||
/*乐迪反馈值转换(-1 -- 1 )范围*/
|
||||
if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
|
||||
else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
|
||||
|
||||
@ -42,7 +55,25 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
||||
|
||||
cmd->key_ctrl_l = rc->key [0];
|
||||
cmd->key_ctrl_r = rc->key [1];
|
||||
#elif defined(dr16_t)
|
||||
|
||||
cmd->Vx = rc->ch_r_x;
|
||||
cmd->Vy = -rc->ch_r_y;
|
||||
cmd->Vw = rc->ch_l_x;
|
||||
|
||||
cmd->poscamear = rc->ch_l_y;
|
||||
|
||||
cmd->key_ctrl_l = rc->sw_l;
|
||||
cmd->key_ctrl_r = rc->sw_r ;
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -59,12 +90,15 @@ static void CMD_RcLostLogic(CMD_t *cmd){
|
||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
|
||||
if (cmd == NULL) return -1;
|
||||
if (rc == NULL) return -1;
|
||||
#ifdef dr16_t
|
||||
/*c当rc丢控时，恢复机器人至默认状态 */
|
||||
// if ((rc->ch_w == CMD_SW_ERR) || (rc->ch_y == CMD_SW_ERR)) {
|
||||
// CMD_RcLostLogic(cmd);
|
||||
// } else {
|
||||
if ((rc->ch_w == CMD_SW_ERR) || (rc->ch_y == CMD_SW_ERR)) {
|
||||
CMD_RcLostLogic(cmd);
|
||||
} else {
|
||||
CMD_RcLogic(rc, cmd);
|
||||
// }
|
||||
}
|
||||
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -174,27 +208,4 @@ int8_t CMD_CtrlSet(CMD_t *cmd) {
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
|
||||
{
|
||||
if(cmd == NULL) return -1;
|
||||
if(act == NULL) return -1;
|
||||
|
||||
if(cmd->key_ctrl_l ==CMD_SW_MID )
|
||||
{
|
||||
if(cmd->key_ctrl_r == CMD_SW_UP)
|
||||
act->flag =1;
|
||||
|
||||
if(cmd->key_ctrl_r == CMD_SW_DOWN )
|
||||
act->flag =-1;
|
||||
|
||||
}
|
||||
|
||||
cmd->C_navi .vx =act ->out .Vx ;
|
||||
cmd ->C_navi .vy =act ->out .Vy ;
|
||||
cmd ->C_navi .wz =act ->out .Vw ;
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -24,6 +24,14 @@
|
||||
|
||||
#define MID (0x09)
|
||||
|
||||
|
||||
/*选择遥控器，else为r12ds*/
|
||||
//#define dr16_t
|
||||
#define r12ds_t
|
||||
|
||||
|
||||
|
||||
|
||||
typedef enum{
|
||||
RC,//遥控器控制，左按键上
|
||||
MID_NAVI,//雷达导航
|
||||
@ -63,54 +71,46 @@ typedef enum {
|
||||
CMD_SW_DOWN = 2,
|
||||
} CMD_SwitchPos_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
RC_dr16 ,
|
||||
RC_r12ds
|
||||
}remote_type;
|
||||
|
||||
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
|
||||
typedef struct {
|
||||
// float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
|
||||
// float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
|
||||
// float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
|
||||
// float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
|
||||
|
||||
remote_type RC_type;
|
||||
#ifdef dr16_t
|
||||
|
||||
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
|
||||
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
|
||||
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
|
||||
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
|
||||
|
||||
// float ch_res; /* 第五通道值 */
|
||||
float ch_res; /* 第五通道值 */
|
||||
|
||||
// CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
|
||||
// CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
|
||||
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
|
||||
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
|
||||
|
||||
|
||||
// uint16_t key; /* 按键值 */
|
||||
uint16_t key; /* 按键值 */
|
||||
|
||||
// uint16_t res; /* 保留，未启用 */
|
||||
uint16_t res; /* 保留，未启用 */
|
||||
|
||||
#else
|
||||
int16_t ch_x; // X轴（Channel 1）
|
||||
int16_t mul; // MUL（Channel 2）
|
||||
int16_t ch_y; // Y轴（Channel 3）
|
||||
int16_t ch_w; // W轴（Channel 4）
|
||||
int16_t key[8]; // 按键通道（Channel 5-12）
|
||||
|
||||
int16_t offline;
|
||||
#endif
|
||||
} __attribute__((packed))CMD_RC_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 Vx;
|
||||
fp32 Vy;
|
||||
fp32 Vw;
|
||||
}OpsHopeVector_t;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 x;
|
||||
fp32 y;
|
||||
fp32 z;
|
||||
int cnt_point;//计数点
|
||||
}ops_point;
|
||||
|
||||
|
||||
typedef struct {
|
||||
|
||||
OpsHopeVector_t out;
|
||||
|
||||
int flag;//传递flag触发标志位
|
||||
|
||||
} CMD_ACTION_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t status_fromnuc;
|
||||
@ -121,12 +121,7 @@ typedef struct {
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
}navi;
|
||||
struct
|
||||
{
|
||||
fp32 posx;
|
||||
fp32 posy;
|
||||
fp32 posw;
|
||||
}pick;
|
||||
|
||||
struct
|
||||
{
|
||||
fp32 angle;
|
||||
@ -175,11 +170,8 @@ int8_t CMD_Init(CMD_t *cmd);
|
||||
|
||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
|
||||
|
||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act);
|
||||
|
||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
|
||||
|
||||
|
||||
int8_t CMD_SwitchStatus(CMD_t *cmd);
|
||||
|
||||
int8_t CMD_CtrlSet(CMD_t *cmd);
|
||||
|
||||
@ -1,135 +1,137 @@
|
||||
///*
|
||||
// DR16接收机
|
||||
/*
|
||||
DR16接收机
|
||||
|
||||
//*/
|
||||
*/
|
||||
|
||||
///* Includes ----------------------------------------------------------------- */
|
||||
//#include "dr16.h"
|
||||
|
||||
//#include <string.h>
|
||||
|
||||
//#include "bsp_usart.h"
|
||||
//#include "error_detect.h"
|
||||
///* Private define ----------------------------------------------------------- */
|
||||
//#define DR16_CH_VALUE_MIN (364u)
|
||||
//#define DR16_CH_VALUE_MID (1024u)
|
||||
//#define DR16_CH_VALUE_MAX (1684u)
|
||||
|
||||
///* Private macro ------------------------------------------------------------ */
|
||||
///* Private typedef ---------------------------------------------------------- */
|
||||
///* Private variables -------------------------------------------------------- */
|
||||
|
||||
//static osThreadId_t thread_alert;
|
||||
//uint8_t cbuf[36]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
|
||||
///* Private function -------------------------------------------------------- */
|
||||
//static void DR16_RxCpltCallback(void) {
|
||||
// osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
|
||||
//// detect_hook(DR16_TOE);
|
||||
//}
|
||||
|
||||
//static bool DR16_DataCorrupted(const DR16_t *dr16) {
|
||||
// if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
|
||||
// (dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
|
||||
// return true;
|
||||
|
||||
// if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
|
||||
// (dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
|
||||
// return true;
|
||||
|
||||
// if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
|
||||
// (dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
|
||||
// return true;
|
||||
|
||||
// if ((dr16->data.ch_l_y < 1000u) ||
|
||||
// (dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
|
||||
// return true;
|
||||
|
||||
// if (dr16->data.sw_l == 0) return true;
|
||||
|
||||
// if (dr16->data.sw_r == 0) return true;
|
||||
|
||||
// return false;
|
||||
//}
|
||||
///* Exported functions ------------------------------------------------------- */
|
||||
//int8_t DR16_Init(DR16_t *dr16) {
|
||||
// if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
|
||||
// DR16_RxCpltCallback);
|
||||
// return DEVICE_OK;
|
||||
//}
|
||||
|
||||
//int8_t DR16_Restart(void) {
|
||||
// __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||||
// __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||||
// return DEVICE_OK;
|
||||
//}
|
||||
|
||||
//int8_t DR16_StartDmaRecv(DR16_t *dr16) {
|
||||
// if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
|
||||
// (uint8_t *)cbuf,
|
||||
// sizeof(cbuf)) == HAL_OK)
|
||||
// return DEVICE_OK;
|
||||
// return DEVICE_ERR;
|
||||
//}
|
||||
|
||||
//bool DR16_WaitDmaCplt(uint32_t timeout) {
|
||||
// return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
|
||||
// SIGNAL_DR16_RAW_REDY);
|
||||
//}
|
||||
|
||||
//int8_t DR16_ParseRaw(DR16_t *dr16)
|
||||
//{
|
||||
// if(dr16 ==NULL) return DEVICE_ERR_NULL;
|
||||
// dr16->data.ch_r_x = (cbuf[0] | (cbuf[1] <<8));
|
||||
// dr16->data.ch_r_y = ((cbuf[1] >> 3)| (cbuf[2] << 5));
|
||||
// dr16->data.ch_l_x = ((cbuf[2] >>6) | (cbuf[3] << 2) | (cbuf[4] <<10));
|
||||
// dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
|
||||
// dr16->data.sw_r = ((cbuf[5] >>4));
|
||||
// dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;
|
||||
//
|
||||
// return 1;
|
||||
//}
|
||||
//int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc) {
|
||||
// if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
//
|
||||
// if (DR16_DataCorrupted(dr16)) {
|
||||
// return DEVICE_ERR;
|
||||
// } else {
|
||||
// memset(rc, 0, sizeof(*rc));
|
||||
// }
|
||||
|
||||
// float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
|
||||
|
||||
// rc->ch_r_x = 2 * ((float)dr16->data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
|
||||
// rc->ch_r_y = 2 * ((float)dr16->data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
|
||||
// rc->ch_l_x = 2 * ((float)dr16->data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
|
||||
// rc->ch_l_y = 2 * ((float)dr16->data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
|
||||
|
||||
// rc->sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
|
||||
// rc->sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
|
||||
|
||||
// rc->key = dr16->data.key;
|
||||
|
||||
//// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
|
||||
// return DEVICE_OK;
|
||||
//}
|
||||
|
||||
//int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
|
||||
// if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
// if (rc == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// (void)dr16;
|
||||
// memset(rc, 0, sizeof(*rc));
|
||||
// return 0;
|
||||
//}
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "dr16.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "bsp_usart.h"
|
||||
#include "error_detect.h"
|
||||
|
||||
|
||||
#ifdef dr16_t
|
||||
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
#define DR16_CH_VALUE_MIN (364u)
|
||||
#define DR16_CH_VALUE_MID (1024u)
|
||||
#define DR16_CH_VALUE_MAX (1684u)
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
uint8_t cbuf[36]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
|
||||
/* Private function -------------------------------------------------------- */
|
||||
static void DR16_RxCpltCallback(void) {
|
||||
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
|
||||
// detect_hook(DR16_TOE);
|
||||
}
|
||||
|
||||
static bool DR16_DataCorrupted(const DR16_t *dr16) {
|
||||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) ||
|
||||
(dr16->data.ch_r_x > DR16_CH_VALUE_MAX))
|
||||
return true;
|
||||
|
||||
if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) ||
|
||||
(dr16->data.ch_r_y > DR16_CH_VALUE_MAX))
|
||||
return true;
|
||||
|
||||
if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) ||
|
||||
(dr16->data.ch_l_x > DR16_CH_VALUE_MAX))
|
||||
return true;
|
||||
|
||||
if ((dr16->data.ch_l_y < 1000u) ||
|
||||
(dr16->data.ch_l_y > DR16_CH_VALUE_MAX))
|
||||
return true;
|
||||
|
||||
if (dr16->data.sw_l == 0) return true;
|
||||
|
||||
if (dr16->data.sw_r == 0) return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t DR16_Init(DR16_t *dr16) {
|
||||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
|
||||
DR16_RxCpltCallback);
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t DR16_Restart(void) {
|
||||
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||||
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t DR16_StartDmaRecv(DR16_t *dr16) {
|
||||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
|
||||
(uint8_t *)cbuf,
|
||||
sizeof(cbuf)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
bool DR16_WaitDmaCplt(uint32_t timeout) {
|
||||
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
|
||||
SIGNAL_DR16_RAW_REDY);
|
||||
}
|
||||
|
||||
int8_t DR16_ParseRaw(DR16_t *dr16)
|
||||
{
|
||||
if(dr16 ==NULL) return DEVICE_ERR_NULL;
|
||||
dr16->data.ch_r_x = (cbuf[0] | (cbuf[1] <<8));
|
||||
dr16->data.ch_r_y = ((cbuf[1] >> 3)| (cbuf[2] << 5));
|
||||
dr16->data.ch_l_x = ((cbuf[2] >>6) | (cbuf[3] << 2) | (cbuf[4] <<10));
|
||||
dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
|
||||
dr16->data.sw_r = ((cbuf[5] >>4));
|
||||
dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int8_t DR16_ParseRC(const DR16_t *dr16, CMD_RC_t *rc) {
|
||||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
if (DR16_DataCorrupted(dr16)) {
|
||||
return DEVICE_ERR;
|
||||
} else {
|
||||
memset(rc, 0, sizeof(*rc));
|
||||
}
|
||||
|
||||
float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
|
||||
|
||||
rc->ch_r_x = 2 * ((float)dr16->data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
|
||||
rc->ch_r_y = 2 * ((float)dr16->data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
|
||||
rc->ch_l_x = 2 * ((float)dr16->data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
|
||||
rc->ch_l_y = 2 * ((float)dr16->data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
|
||||
|
||||
rc->sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
|
||||
rc->sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
|
||||
|
||||
rc->key = dr16->data.key;
|
||||
|
||||
// rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
|
||||
if (dr16 == NULL) return DEVICE_ERR_NULL;
|
||||
if (rc == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
(void)dr16;
|
||||
memset(rc, 0, sizeof(*rc));
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -11,6 +11,11 @@
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
#ifdef dr16_t
|
||||
|
||||
|
||||
#define dr16_t
|
||||
typedef __packed struct
|
||||
{
|
||||
uint16_t ch_r_x : 11;
|
||||
@ -44,3 +49,4 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc);
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@ -12,6 +12,8 @@
|
||||
#include <string.h>
|
||||
#include "cmd.h"
|
||||
#include "user_math.h"
|
||||
|
||||
#ifdef r12ds_t
|
||||
extern UART_HandleTypeDef huart3;
|
||||
extern DMA_HandleTypeDef hdma_usart3_rx;
|
||||
|
||||
@ -121,11 +123,6 @@ int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
||||
return 1;
|
||||
}
|
||||
|
||||
//int map(int x, int in_min, int in_max, int out_min, int out_max) //ӳʤگ˽
|
||||
//{
|
||||
// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
//}
|
||||
|
||||
|
||||
int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
|
||||
{
|
||||
@ -180,5 +177,5 @@ int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -6,6 +6,9 @@
|
||||
#include "bsp_usart.h"
|
||||
#include "cmd.h"
|
||||
|
||||
|
||||
#ifdef r12ds_t
|
||||
|
||||
#define RC_FRAME_LENGTH 25u
|
||||
|
||||
|
||||
@ -21,8 +24,8 @@ int8_t sbus_to_rc(volatile const uint8_t *sbus_buf,CMD_RC_t *rc_ctrl);
|
||||
//void R12ds_HandleOffline(void);
|
||||
|
||||
|
||||
// int map_int(int x, int in_min, int in_max, int out_min, int out_max) ;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,75 +0,0 @@
|
||||
/*
|
||||
码盘解析任务
|
||||
|
||||
处理码盘测量的实际距离
|
||||
|
||||
将需要的数据共享给其他的线程
|
||||
|
||||
*/
|
||||
#include "action_task.h"
|
||||
#include "Action.h"
|
||||
#include "user_task.h"
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
Action_POS_t pos;
|
||||
#else
|
||||
|
||||
static Action_POS pos;
|
||||
|
||||
#endif
|
||||
|
||||
void Task_action(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
// osDelay(TASK_INIT_DELAY_ACTION);
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION;
|
||||
|
||||
|
||||
ACTIONRECV_Init(&pos);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
||||
|
||||
while (1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
/* 记录任务使用的的栈空闄*/
|
||||
task_runtime.stack_water_mark.action =
|
||||
osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
ACTION_StartReceiving();
|
||||
ACTION_DataRefresh(&pos);
|
||||
|
||||
|
||||
|
||||
/* 使用下面的错误处理函数遇到一些问题
|
||||
每10ms置标志位， ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算
|
||||
根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作
|
||||
目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset
|
||||
|
||||
|
||||
*/
|
||||
|
||||
if(ACTION_WaitDmaCplt()){
|
||||
|
||||
ACTION_Parse(&pos);
|
||||
|
||||
}
|
||||
else{
|
||||
Action_HandleOffline(&pos);
|
||||
}
|
||||
|
||||
|
||||
//将解算后的码盘位置值放入消息队列供其他任务使用
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.Action);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0);
|
||||
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1 +0,0 @@
|
||||
|
||||
@ -13,8 +13,6 @@
|
||||
static CAN_t can;
|
||||
|
||||
|
||||
#define remote_mode dr16 //遥控器选择
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
CAN_Output_t out;
|
||||
@ -70,9 +68,7 @@ void Task_Chassis(void *argument)
|
||||
osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
|
||||
/*can上设备数据获取*/
|
||||
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
|
||||
|
||||
|
||||
/*底盘控制信息获取,目前dji遥控器*/
|
||||
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
|
||||
|
||||
|
||||
@ -6,7 +6,6 @@
|
||||
CMD_t cmd;
|
||||
CMD_RC_t rc_ctrl;
|
||||
CMD_NUC_t Nuc;
|
||||
CMD_ACTION_t cmd_ops_out;
|
||||
|
||||
#else
|
||||
|
||||
@ -52,8 +51,6 @@ void Task_cmd(void *argument){
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||
CMD_ParseRc(&cmd, &rc_ctrl);
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘
|
||||
CMD_ParseAction (&cmd ,&cmd_ops_out);
|
||||
osKernelUnlock(); /* 同上 解锁RTOS内核 */
|
||||
|
||||
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
|
||||
|
||||
@ -1,104 +1,62 @@
|
||||
///*
|
||||
// DR16接收机通信任务（支持DR16和SBUS双协议）
|
||||
//*/
|
||||
/*
|
||||
DR16接收机通信任务
|
||||
*/
|
||||
|
||||
///* Includes ----------------------------------------------------------------- */
|
||||
//#include <string.h>
|
||||
//#include "dr16.h"
|
||||
//#include "LD_remote.h" // 新增 SBUS 支持
|
||||
//#include "user_task.h"
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <string.h>
|
||||
|
||||
///* Private typedef ---------------------------------------------------------- */
|
||||
///* Private define ----------------------------------------------------------- */
|
||||
//#define PROTOCOL_DR16 0
|
||||
//#define PROTOCOL_SBUS 1
|
||||
#include "dr16.h"
|
||||
#include "user_task.h"
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
///* Private variables -------------------------------------------------------- */
|
||||
//#ifdef DEBUG
|
||||
//DR16_t dr16;
|
||||
//CMD_RC_t cmd_rc;
|
||||
//#else
|
||||
//static DR16_t dr16;
|
||||
//static CMD_RC_t cmd_rc;
|
||||
//#endif
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
|
||||
//static uint8_t protocol_type = PROTOCOL_DR16; // 当前协议类型
|
||||
// uint8_t sbus_rx_buf[36]; // SBUS 数据缓冲区
|
||||
#ifdef dr16
|
||||
|
||||
///* Private function --------------------------------------------------------- */
|
||||
///**
|
||||
// * \brief 协议识别函数
|
||||
// *
|
||||
// * \param buf 接收到的数据缓冲区
|
||||
// * \return uint8_t 协议类型：PROTOCOL_DR16 或 PROTOCOL_SBUS
|
||||
// */
|
||||
//static uint8_t Detect_Protocol(const uint8_t *buf) {
|
||||
// if (buf[0] == 0x0F && buf[24] == 0x00) { // SBUS 协议特征
|
||||
// return PROTOCOL_SBUS;
|
||||
// }
|
||||
// return PROTOCOL_DR16; // 默认 DR16
|
||||
//}
|
||||
#ifdef DEBUG
|
||||
DR16_t dr16;
|
||||
CMD_RC_t cmd_rc;
|
||||
#else
|
||||
static DR16_t dr16;
|
||||
static CMD_RC_t cmd_rc;
|
||||
#endif
|
||||
|
||||
///**
|
||||
// * \brief 统一回调函数
|
||||
// */
|
||||
//static void UART_RxCpltCallback(void) {
|
||||
// if (Detect_Protocol(sbus_rx_buf) == PROTOCOL_SBUS) {
|
||||
// protocol_type = PROTOCOL_SBUS;
|
||||
// } else {
|
||||
// protocol_type = PROTOCOL_DR16;
|
||||
// }
|
||||
// osThreadFlagsSet(osThreadGetId(), SIGNAL_DR16_RAW_REDY);
|
||||
//}
|
||||
#endif
|
||||
|
||||
///* Exported functions ------------------------------------------------------- */
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
|
||||
///**
|
||||
// * \brief dr16接收机任务（支持DR16和SBUS双协议）
|
||||
// *
|
||||
// * \param argument 未使用
|
||||
// */
|
||||
//void Task_dr16(void *argument) {
|
||||
// (void)argument; /* 未使用，消除警告 */
|
||||
/**
|
||||
* \brief dr16接收机
|
||||
*
|
||||
* \param argument 未使用
|
||||
*/
|
||||
void Task_dr16(void *argument) {
|
||||
(void)argument; /* 未使用，消除警告 */
|
||||
#ifdef dr16
|
||||
|
||||
// DR16_Init(&dr16); /* 初始化 DR16 */
|
||||
// RC_New_Init(); /* 初始化 SBUS */
|
||||
DR16_Init(&dr16); /* 初始化dr16 */
|
||||
|
||||
// while (1) {
|
||||
//#ifdef DEBUG
|
||||
// /* 调试信息：堆栈水位 */
|
||||
// task_runtime.stack_water_mark.dr16 = osThreadGetStackSpace(osThreadGetId());
|
||||
//#endif
|
||||
while (1) {
|
||||
#ifdef DEBUG
|
||||
/* */
|
||||
task_runtime.stack_water_mark.dr16 = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
/* 开启DMA */
|
||||
DR16_StartDmaRecv(&dr16);
|
||||
|
||||
// /* 开启 DMA 接收 */
|
||||
// if (protocol_type == PROTOCOL_DR16) {
|
||||
// DR16_StartDmaRecv(&dr16);
|
||||
// } else {
|
||||
// HAL_UARTEx_ReceiveToIdle_DMA(&huart3, sbus_rx_buf, sizeof(sbus_rx_buf));
|
||||
// }
|
||||
|
||||
// /* 等待 DMA 完成 */
|
||||
// if (DR16_WaitDmaCplt(30)) {
|
||||
// if (protocol_type == PROTOCOL_DR16) {
|
||||
// /* DR16 数据处理 */
|
||||
// DR16_ParseRaw(&dr16);
|
||||
// DR16_ParseRC(&dr16, &cmd_rc);
|
||||
// } else {
|
||||
// /* SBUS 数据处理 */
|
||||
// const RC_Ctrl_New_t *sbus_data = RC_New_GetData();
|
||||
// cmd_rc.ch_r_x = sbus_data->ch[0] / 1000.0f; // 映射到 [-1, 1]
|
||||
// cmd_rc.ch_r_y = sbus_data->ch[1] / 1000.0f;
|
||||
// cmd_rc.ch_l_x = sbus_data->ch[2] / 1000.0f;
|
||||
// cmd_rc.ch_l_y = sbus_data->ch[3] / 1000.0f;
|
||||
// }
|
||||
// } else {
|
||||
// /* 处理遥控器离线 */
|
||||
// DR16_HandleOffline(&dr16, &cmd_rc);
|
||||
// }
|
||||
|
||||
// /* 发送数据到消息队列 */
|
||||
// osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
|
||||
// osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
|
||||
// }
|
||||
//}
|
||||
if (DR16_WaitDmaCplt(30)) {
|
||||
/* 转换 */
|
||||
DR16_ParseRaw(&dr16);
|
||||
DR16_ParseRC(&dr16, &cmd_rc);
|
||||
} else {
|
||||
/* 处理遥控器离线 */
|
||||
DR16_HandleOffline(&dr16, &cmd_rc);
|
||||
}
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -11,7 +11,6 @@
|
||||
#include "task\user_task.h"
|
||||
#include "can_use.h"
|
||||
#include "cmd.h"
|
||||
#include "Action.h"
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
@ -36,8 +35,6 @@ void Task_Init(void *argument) {
|
||||
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.chassis =
|
||||
osThreadNew(Task_Chassis, NULL, &attr_chassis);
|
||||
// task_runtime.thread.dr16 =
|
||||
// osThreadNew(Task_dr16,NULL,&attr_dr16);
|
||||
task_runtime.thread.r12ds =
|
||||
osThreadNew(Task_r12ds,NULL,&attr_r12ds);
|
||||
|
||||
@ -47,13 +44,7 @@ void Task_Init(void *argument) {
|
||||
osThreadNew(Task_cmd,NULL,&attr_cmd);
|
||||
task_runtime.thread.nuc =
|
||||
osThreadNew(Task_nuc,NULL,&attr_nuc);
|
||||
|
||||
task_runtime.thread.navi =
|
||||
osThreadNew(Task_Navi,NULL,&attr_navi);
|
||||
|
||||
task_runtime.thread.action_ops=
|
||||
osThreadNew(Task_action,NULL,&attr_ops_9pos);
|
||||
|
||||
task_runtime.thread.error_detect =
|
||||
osThreadNew(Task_error_detect,NULL,&attr_error_detect);
|
||||
|
||||
@ -88,13 +79,6 @@ void Task_Init(void *argument) {
|
||||
osMessageQueueNew(3u, sizeof(CMD_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.nuc =
|
||||
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.Action =
|
||||
osMessageQueueNew(3u,sizeof(Action_POS_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.ops_9_out =
|
||||
osMessageQueueNew(3u,sizeof(CMD_ACTION_t), NULL);
|
||||
|
||||
// task_runtime.mutex.imu_eulr =
|
||||
// xSemaphoreCreateMutex();
|
||||
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
|
||||
@ -1,68 +0,0 @@
|
||||
#include "user_task.h"
|
||||
#include "navi.h"
|
||||
#include "map.h"
|
||||
|
||||
#ifdef DEBUG
|
||||
ops_t o;
|
||||
Action_POS_t pos_get;
|
||||
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
|
||||
|
||||
#else
|
||||
|
||||
static ops_t o;
|
||||
static Action_POS_t pos_get;
|
||||
static CMD_ACTION_t ops_out;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
void Task_Navi(void *argument){
|
||||
|
||||
(void)argument;
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
|
||||
|
||||
Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
||||
|
||||
while (1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
/* 记录任务使用的的栈空间 */
|
||||
task_runtime.stack_water_mark.action =
|
||||
osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
|
||||
|
||||
go_path(&o,&ops_out);
|
||||
|
||||
//将解算后的导航期望运动值放入消息队列供其他任务使用
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
|
||||
|
||||
@ -1 +0,0 @@
|
||||
#include "navi.h"
|
||||
@ -3,6 +3,9 @@
|
||||
#include "user_task.h"
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#ifdef r12ds_t
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
CMD_RC_t cmd_rc;
|
||||
@ -20,6 +23,8 @@ static CMD_RC_t cmd_rc;
|
||||
CMD_RC_t *R12ds_DataGet(){
|
||||
return &cmd_rc;
|
||||
}
|
||||
|
||||
|
||||
//void R12ds_HandleOffline(void) {
|
||||
// CMD_RC_t *rc;
|
||||
// rc = R12ds_DataGet();
|
||||
@ -27,10 +32,15 @@ CMD_RC_t *R12ds_DataGet(){
|
||||
// memset(rc, 0, sizeof(*rc));
|
||||
// rc->offline = 1;//用作遥控器断电后急停使用
|
||||
//}
|
||||
|
||||
#endif
|
||||
|
||||
void Task_r12ds(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
|
||||
#ifdef r12ds_t
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS;
|
||||
R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50);
|
||||
|
||||
@ -61,6 +71,7 @@ void Task_r12ds(void *argument)
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -25,11 +25,6 @@ const osThreadAttr_t attr_atti_esti = {
|
||||
.priority = osPriorityRealtime,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ops_9pos = {
|
||||
.name = "action",
|
||||
.priority = osPriorityRealtime,
|
||||
.stack_size = 256 *4,
|
||||
};
|
||||
const osThreadAttr_t attr_chassis = {
|
||||
.name = "chassis",
|
||||
.priority = osPriorityAboveNormal,
|
||||
@ -65,14 +60,10 @@ const osThreadAttr_t attr_error_detect = {
|
||||
.stack_size = 128 *4,
|
||||
};
|
||||
|
||||
//const osThreadAttr_t attr_dr16 = {
|
||||
// .name = "dr16",
|
||||
// .priority = osPriorityRealtime,
|
||||
// .stack_size = 128 *4,
|
||||
//};
|
||||
|
||||
const osThreadAttr_t attr_navi = {
|
||||
.name = "navi",
|
||||
const osThreadAttr_t attr_dr16 = {
|
||||
.name = "dr16",
|
||||
.priority = osPriorityRealtime,
|
||||
.stack_size = 128 *4,
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -18,17 +18,13 @@
|
||||
#define TASK_FREQ_CTRL_CMD (500u)
|
||||
#define TASK_FREQ_NUC (500u)
|
||||
#define TASK_FREQ_CAN (1000u)
|
||||
#define TASK_FREQ_NAVI (500u)
|
||||
#define TASK_FREQ_R12DS (1000u)
|
||||
#define TASK_FREQ_ACTION (500u)
|
||||
#define TASK_FREQ_dr16 (1000u)
|
||||
|
||||
#define TASK_FREQ_ERROR_DTC (3u)
|
||||
|
||||
#define DEBUG
|
||||
|
||||
|
||||
#define TASK_INIT_DELAY_ACTION (400u)
|
||||
|
||||
#define TASK_INIT_DELAY_NUC (500u)
|
||||
|
||||
/* Exported defines --------------------------------------------------------- */
|
||||
@ -40,17 +36,14 @@ typedef struct {
|
||||
struct {
|
||||
osThreadId_t atti_esti;
|
||||
osThreadId_t chassis;
|
||||
// osThreadId_t dr16;
|
||||
osThreadId_t dr16;
|
||||
|
||||
osThreadId_t r12ds;
|
||||
|
||||
osThreadId_t action_ops;
|
||||
|
||||
osThreadId_t ai;
|
||||
osThreadId_t can;
|
||||
osThreadId_t cmd;
|
||||
osThreadId_t nuc;
|
||||
osThreadId_t navi;
|
||||
osThreadId_t error_detect;
|
||||
} thread;
|
||||
|
||||
@ -67,8 +60,6 @@ typedef struct {
|
||||
osMessageQueueId_t rc;
|
||||
osMessageQueueId_t nuc;
|
||||
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
|
||||
osMessageQueueId_t Action;
|
||||
osMessageQueueId_t ops_9_out;
|
||||
|
||||
|
||||
}raw;
|
||||
@ -105,13 +96,11 @@ typedef struct {
|
||||
UBaseType_t chassis;
|
||||
UBaseType_t can;
|
||||
UBaseType_t atti_esti;
|
||||
// UBaseType_t dr16;
|
||||
UBaseType_t dr16;
|
||||
UBaseType_t r12ds;
|
||||
|
||||
UBaseType_t action;
|
||||
UBaseType_t cmd;
|
||||
UBaseType_t nuc;
|
||||
UBaseType_t navi;
|
||||
UBaseType_t error_detect;
|
||||
|
||||
} stack_water_mark; /* stack使用 */
|
||||
@ -123,8 +112,6 @@ typedef struct {
|
||||
float r12ds;
|
||||
float cmd;
|
||||
float nuc;
|
||||
float action;
|
||||
float navi;
|
||||
float error_detect;
|
||||
} freq; /* 任务运行频率 */
|
||||
|
||||
@ -132,12 +119,9 @@ typedef struct {
|
||||
float chassis;
|
||||
float can;
|
||||
float atti_esti;
|
||||
float ai;
|
||||
float r12ds;
|
||||
float cmd;
|
||||
float nuc;
|
||||
float navi;
|
||||
float action;
|
||||
float error_detect;
|
||||
} last_up_time; /* 任务上一次运行的时间 */
|
||||
#endif
|
||||
@ -155,8 +139,6 @@ extern const osThreadAttr_t attr_can;
|
||||
|
||||
extern const osThreadAttr_t attr_cmd;
|
||||
|
||||
extern const osThreadAttr_t attr_ops_9pos;
|
||||
|
||||
extern const osThreadAttr_t attr_nuc;
|
||||
|
||||
extern const osThreadAttr_t attr_error_detect;
|
||||
@ -164,7 +146,6 @@ extern const osThreadAttr_t attr_error_detect;
|
||||
//extern const osThreadAttr_t attr_dr16;
|
||||
|
||||
extern const osThreadAttr_t attr_r12ds;
|
||||
extern const osThreadAttr_t attr_navi;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
void Task_Init(void *argument);
|
||||
@ -179,14 +160,10 @@ void Task_cmd(void *argument);
|
||||
|
||||
void Task_nuc(void *argument);
|
||||
|
||||
void Task_action(void *argument);
|
||||
|
||||
void Task_Navi(void *argument);
|
||||
|
||||
|
||||
|
||||
void Task_error_detect(void *argument);
|
||||
|
||||
void Task_r12ds(void *argument);
|
||||
|
||||
//void Task_dr16(void *argument);
|
||||
void Task_dr16(void *argument);
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user