zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

图传+限位，抬升问题

This commit is contained in:
zyx 2024-05-09 21:33:08 +08:00
parent bde9237826
commit 04b3942c4d
8 changed files with 63 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
application/cmd/robot_cmd.c
View File

@ -116,6 +116,13 @@ static void CalcOffsetAngle() {
#endif
}
static void death_check()
{
if(referee_data->GameRobotState.remain_HP)
{
gimbal_cmd_send.MotorEnableFlag=5;
}
}
/**
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
*
@ -128,7 +135,7 @@ static void update_ui_data() {
// 出招表
static void RemoteControlSet() {
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动,伸缩保持不动
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
@ -137,8 +144,11 @@ static void RemoteControlSet() {
chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩
to_stretch_cmd_send.tc += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//图传
// 图传限位
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;
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
@ -149,13 +159,14 @@ static void RemoteControlSet() {
to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l_;//抬升
to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//前伸
//伸缩限位待添加
// 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;
}
// 右侧开关状态为[下],机械臂
if (switch_is_down(rc_data[TEMP].rc.switch_right)) {
if (switch_is_up(rc_data[TEMP].rc.switch_right)) {
// 左侧开关状态为[下],前三轴
if(switch_is_down(rc_data[TEMP].rc.switch_left))
{

12
application/gimbal/gimbal.cpp
View File

@ -220,12 +220,22 @@ void GimbalInit()
};
PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
//static void DMMotroEnable()
//{
// if(gimbal_cmd_recv.MotorEnableFlag)
// {
// DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor);
// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor );
// DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor );
// }
//}
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()

8
application/robot.c
View File

@ -31,8 +31,8 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit();
GimbalInit();
//To_stretchInit();
//GimbalInit();
To_stretchInit();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
@ -49,8 +49,8 @@ void RobotTask()
{
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask();
GimbalTask();
//To_stretchTask();
//GimbalTask();
To_stretchTask();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)

5
application/robot_def.h
View File

@ -35,9 +35,6 @@
#define YAW 60
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
@ -150,7 +147,7 @@ typedef struct
float diff_pitch; //差速器pitch
float diff_roll; //差速器roll
float chassis_rotate_wz;
uint8_t MotorEnableFlag;
gimbal_mode_e gimbal_mode;
} Gimbal_Ctrl_Cmd_s;

27
application/to_stretch/to_stretch.c
View File

@ -25,6 +25,7 @@ float l_lift = 0,r_lift = 0;
float lift_z = 0; //抬升高度
float ld_offset = 0,rd_offset = 0;
float lu_offset = 0,ru_offset = 0;
float kp=4,ki=1,kd=0;//调试用
PIDInstance protract_position_loop;//前伸位置环
PIDInstance lift_position_loop;//抬升位置环
@ -45,24 +46,23 @@ void To_stretchInit() {
.MaxOut = 500,
},
.speed_PID = {
.Kp = 2,
.Kp = 4,
.Ki = 1,
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 5000,
.MaxOut = 13000,
},
.current_feedforward_ptr = &gravity_feedforward,
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP,//ANGLE_LOOP,
.close_loop_type = SPEED_LOOP,//SPEED_LOOP | ANGLE_LOOP,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP,
.feedforward_flag = CURRENT_FEEDFORWARD,
},
.motor_type = M3508,
};
// 前后
Motor_Init_Config_s fb_config = {
@ -123,7 +123,7 @@ void To_stretchInit() {
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 20000,
.MaxOut = 15000,
};
PIDInit(&protract_position_loop,&protract_pid_config);
@ -133,14 +133,14 @@ void To_stretchInit() {
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 20000,
.MaxOut = 15000,
};
PIDInit(&lift_position_loop,&lift_pid_config);
// 图传电机
Motor_Init_Config_s tuchuan_config = {
.can_init_config = {
.can_handle = &hcan2,
.can_handle = &hcan1,
.tx_id = 1,
},
.controller_param_init_config = {
@ -148,7 +148,7 @@ void To_stretchInit() {
.Kp = 20,
.Ki = 10,
.Kd = 0,
.MaxOut = 15000,
.MaxOut = 5000,
},
.speed_PID = {
.Kp = 2.0f,
@ -156,7 +156,7 @@ void To_stretchInit() {
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 30000,
.MaxOut = 5000,
},
},
.controller_setting_init_config = {
@ -178,6 +178,10 @@ void To_stretchInit() {
/* 机器人伸缩控制核心任务 */
void To_stretchTask()
{
motor_lu->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp;
motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki;
motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd;
static uint8_t init_flag = FALSE;
if(init_flag == FALSE)
{
@ -235,13 +239,11 @@ void To_stretchTask()
DJIMotorEnable(motor_ru);
DJIMotorEnable(motor_ld);
DJIMotorEnable(motor_rd);
DJIMotorEnable(tuchuan);
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);
@ -249,6 +251,7 @@ void To_stretchTask()
DJIMotorEnable(motor_ld);
DJIMotorEnable(motor_rd);
DJIMotorEnable(tuchuan);
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
break;
default:
break;

35
engineering.jdebug.user
View File

@ -1,30 +1,25 @@
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="gimbal.cpp", FilePath="D:/zhandui/cqdm/engineering/application/gimbal/gimbal.cpp", Line=234
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=48
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=603, h=387, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=1011, h=229, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=523, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=788, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1375, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=383, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=1011, h=946, 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="784;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="800;0"
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=276, h=245, 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;1246]
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";"Access"], ColWidths=[257;124;104;118;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;26]
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

2
modules/motor/DMmotor/dmmotor.c
View File

@ -25,7 +25,7 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id

2
modules/motor/DMmotor/dmmotor.h
View File

@ -68,7 +68,7 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
void DMMotorEnable(DMMotorInstance *motor);
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor);
void DMMotorCaliEncoder(DMMotorInstance *motor);
void DMMotorControlInit();