图传+限位
This commit is contained in:
zyx
parent
df4892df01
commit
e2e32f20df
|
|
@ -45,7 +45,7 @@ static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数
|
|||
|
||||
|
||||
static SuperCapInstance *cap; // 超级电容
|
||||
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
|
||||
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb;
|
||||
float chassis_power = 0;
|
||||
static first_order_filter_type_t vx_filter;
|
||||
static first_order_filter_type_t vy_filter;
|
||||
|
|
@ -111,7 +111,6 @@ void ChassisInit()
|
|||
first_order_filter_init(&vy_filter,0.002f,&chassis_y_order_filter);
|
||||
first_order_filter_init(&vx_filter,0.002f,&chassis_x_order_filter);
|
||||
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
|
|
|
|||
|
|
@ -128,8 +128,7 @@ static void RemoteControlSet() {
|
|||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
|
||||
|
||||
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩
|
||||
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩，图传自由
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
|
||||
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
|
||||
|
|
@ -138,6 +137,11 @@ static void RemoteControlSet() {
|
|||
{
|
||||
to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//0.0025f
|
||||
to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//0.0025f
|
||||
to_stretch_cmd_send.tc += 0.025F * (float) rc_data[TEMP].rc.rocker_r_;//0.0025f
|
||||
|
||||
// 图传限位
|
||||
if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
|
||||
if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
|
||||
//伸缩限位待添加
|
||||
// if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
|
||||
// if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@
|
|||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define TUCHUAN_MAX_ANGLE 180 // 图传电机最大角度
|
||||
#define TUCHUAN_MIN_ANGLE 0 // 图传电机最小角度
|
||||
// 发射参数
|
||||
#define ONE_BULLET_DELTA_ANGLE 1933.272 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49，增加为27
|
||||
|
|
@ -139,6 +141,7 @@ typedef struct
|
|||
// cmd发布的伸缩控制数据,由to_stretch订阅
|
||||
typedef struct
|
||||
{
|
||||
float tc;
|
||||
float ud;
|
||||
float fb;
|
||||
to_stretch_mode_e to_stretch_mode;
|
||||
|
|
@ -174,7 +177,9 @@ typedef struct
|
|||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
float gravity_feedforward;
|
||||
float protract_x; //前伸距离
|
||||
float lift_z; //抬升高度
|
||||
} To_stretch_Upload_Data_s;
|
||||
|
||||
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ static Subscriber_t *to_stretch_sub; // 用于订阅伸缩的
|
|||
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv; // 伸缩接收到的控制命令
|
||||
static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据
|
||||
|
||||
static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd;
|
||||
static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan;
|
||||
|
||||
float gravity_feedforward = 3000;
|
||||
float l_protract = 0,r_protract = 0;
|
||||
|
|
@ -96,7 +96,6 @@ void To_stretchInit() {
|
|||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
|
||||
//上下
|
||||
ud_config.can_init_config.tx_id = 5;
|
||||
ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
|
|
@ -138,6 +137,39 @@ void To_stretchInit() {
|
|||
};
|
||||
PIDInit(&lift_position_loop,&lift_pid_config);
|
||||
|
||||
// 图传电机
|
||||
Motor_Init_Config_s tuchuan_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 1,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 20,
|
||||
.Ki = 10,
|
||||
.Kd = 0,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 2.0f,
|
||||
.Ki = 1.0f,
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 30000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = M2006
|
||||
};
|
||||
tuchuan = DJIMotorInit(&tuchuan_config);
|
||||
|
||||
to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
|
||||
to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));
|
||||
|
||||
|
|
@ -177,6 +209,7 @@ void To_stretchTask()
|
|||
DJIMotorStop(motor_ru);
|
||||
DJIMotorStop(motor_ld);
|
||||
DJIMotorStop(motor_rd);
|
||||
DJIMotorStop(tuchuan);
|
||||
}
|
||||
else
|
||||
{ // 正常工作
|
||||
|
|
@ -184,9 +217,10 @@ void To_stretchTask()
|
|||
DJIMotorEnable(motor_ru);
|
||||
DJIMotorEnable(motor_ld);
|
||||
DJIMotorEnable(motor_rd);
|
||||
DJIMotorEnable(tuchuan);
|
||||
}
|
||||
|
||||
// 根据模式设定运动形式
|
||||
// 根据模式设定伸缩+图传运动形式
|
||||
switch (to_stretch_cmd_recv.to_stretch_mode)
|
||||
{
|
||||
case TO_STRETCH_ZERO_FORCE:
|
||||
|
|
@ -194,25 +228,27 @@ void To_stretchTask()
|
|||
DJIMotorStop(motor_ru);
|
||||
DJIMotorStop(motor_ld);
|
||||
DJIMotorStop(motor_rd);
|
||||
DJIMotorStop(tuchuan);
|
||||
break;
|
||||
case TO_STRETCH_MODE:
|
||||
DJIMotorEnable(motor_lu);
|
||||
DJIMotorEnable(motor_ru);
|
||||
DJIMotorEnable(motor_ld);
|
||||
DJIMotorEnable(motor_rd);
|
||||
DJIMotorEnable(tuchuan);
|
||||
|
||||
// DJIMotorSetRef(motor_ld, 1000);
|
||||
// DJIMotorSetRef(motor_ld, 1000);
|
||||
DJIMotorSetRef(motor_lu, ud_speed_set);
|
||||
DJIMotorSetRef(motor_ru, -ud_speed_set);
|
||||
DJIMotorSetRef(motor_ld, -fb_speed_set);
|
||||
DJIMotorSetRef(motor_rd, fb_speed_set);
|
||||
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
|
||||
break;
|
||||
case TO_STRETCH_KEEP:
|
||||
DJIMotorEnable(motor_lu);
|
||||
DJIMotorEnable(motor_ru);
|
||||
DJIMotorEnable(motor_ld);
|
||||
DJIMotorEnable(motor_rd);
|
||||
DJIMotorEnable(tuchuan);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -1,35 +1,27 @@
|
|||
|
||||
|
||||
GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
|
||||
GraphedExpression="(lift_position_loop).Measure", Color=#35792b
|
||||
GraphedExpression="(lift_position_loop).Output", Color=#769dda
|
||||
OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69
|
||||
OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0
|
||||
OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3399
|
||||
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51
|
||||
OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0
|
||||
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=65
|
||||
OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/engineering/application/chassis/chassis.c", Line=94
|
||||
OpenDocument="to_stretch.c", FilePath="D:/zhandui/cqdm/engineering/application/to_stretch/to_stretch.c", Line=215
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=208, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=398, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=470, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1046, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1011, h=679, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;472", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="717;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="238;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=209, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=649, h=340, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=1206, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=546, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=649, h=716, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=948, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=1098, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1206, h=792, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;539", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="716;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="659;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=285, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;770]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure";" (lift_position_loop).Output"], ColWidths=[100;100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;118;118;110;110;100;118;110]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;950]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;110;126;126]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[238;226;104;100]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[257;124;104;100]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;27]
|
||||
WatchedExpression="motor_lf", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="motor_ld", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="motor_rd", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="gravity_feedforward", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="motor_lu", Window=Watched Data 1
|
||||
WatchedExpression="motor_ru", Window=Watched Data 1
|
||||
WatchedExpression="protract_position_loop", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="tuchuan", RefreshRate=2, Window=Watched Data 1
|
||||
WatchedExpression="to_stretch_cmd_recv", RefreshRate=2, Window=Watched Data 1
|
||||
Loading…
Reference in New Issue