diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 535e8c4..df310d0 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -83,7 +83,7 @@ int main(void) /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + HAL_Init(); /* USER CODE BEGIN Init */ diff --git a/Makefile b/Makefile index 7f97471..f6a50ab 100644 --- a/Makefile +++ b/Makefile @@ -141,6 +141,7 @@ modules/can_comm/can_comm.c \ modules/message_center/message_center.c \ modules/daemon/daemon.c \ modules/vofa/vofa.c \ +modules/ps_handle/ps_handle.c \ application/gimbal/gimbal.c \ application/chassis/chassis.c \ application/shoot/shoot.c \ @@ -262,6 +263,7 @@ C_INCLUDES = \ -Imodules/message_center \ -Imodules/daemon \ -Imodules/vofa \ +-Imodules/ps_handle \ -Imodules # compile gcc flags diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 3a6233c..6c59ee2 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -69,6 +69,7 @@ void RobotCMDInit() cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD gimbal_cmd_send.pitch = 0; +gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -100,6 +101,17 @@ static void CalcOffsetAngle() #endif } +static void OffsetAnglePidCalc() +{ + // float pid_measure,pid_ref; + // static PIDInstance AngleCal = { + // .Kp = -1, + // .Ki = 0, + // .Kd = 0, + // .MaxOut = 10000, + // }; + // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0); +} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -134,6 +146,7 @@ static void RemoteControlSet() // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + //chassis_cmd_send.wz = 300; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 @@ -181,6 +194,8 @@ static void EmergencyHandler() gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; // 云台模式改为零力矩 chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘模式改为零力矩 shoot_cmd_send.shoot_mode = SHOOT_OFF; // 射击模式改为关闭 + shoot_cmd_send.friction_mode = FRICTION_OFF; + shoot_cmd_send.load_mode = LOAD_STOP; } // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) @@ -218,7 +233,7 @@ void RobotCMDTask() vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; + vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;; // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 0df5633..f82e2ee 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -6,9 +6,10 @@ #include "ins_task.h" #include "message_center.h" #include "general_def.h" + #include "bmi088.h" -static attitude_t *gimba_IMU_data; // 云台IMU数据 +static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor; // yaw电机 static DJIMotorInstance *pitch_motor; // pitch电机 @@ -17,45 +18,45 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 -BMI088Instance* imu; +BMI088Instance *imu; void GimbalInit() { BMI088_Init_Config_s imu_config = { - .spi_acc_config={ - .GPIOx=CS1_ACCEL_GPIO_Port, - .cs_pin=CS1_ACCEL_Pin, - .spi_handle=&hspi1, + .spi_acc_config = { + .GPIOx = CS1_ACCEL_GPIO_Port, + .cs_pin = CS1_ACCEL_Pin, + .spi_handle = &hspi1, }, - .spi_gyro_config={ - .GPIOx=CS1_GYRO_GPIO_Port, - .cs_pin=CS1_GYRO_Pin, - .spi_handle=&hspi1, + .spi_gyro_config = { + .GPIOx = CS1_GYRO_GPIO_Port, + .cs_pin = CS1_GYRO_Pin, + .spi_handle = &hspi1, }, - .acc_int_config={ - .exti_mode=EXTI_TRIGGER_FALLING, - .GPIO_Pin=INT_ACC_Pin, - .GPIOx=INT_ACC_GPIO_Port, + .acc_int_config = { + .exti_mode = EXTI_TRIGGER_FALLING, + .GPIO_Pin = INT_ACC_Pin, + .GPIOx = INT_ACC_GPIO_Port, }, - .gyro_int_config={ - .exti_mode=EXTI_TRIGGER_FALLING, - .GPIO_Pin=INT_GYRO_Pin, - .GPIOx=INT_GYRO_GPIO_Port, + .gyro_int_config = { + .exti_mode = EXTI_TRIGGER_FALLING, + .GPIO_Pin = INT_GYRO_Pin, + .GPIOx = INT_GYRO_GPIO_Port, }, - .heat_pid_config={ - .Kp=0.0f, - .Kd=0.0f, - .Ki=0.0f, - .MaxOut=0.0f, - .DeadBand=0.0f, + .heat_pid_config = { + .Kp = 0.0f, + .Kd = 0.0f, + .Ki = 0.0f, + .MaxOut = 0.0f, + .DeadBand = 0.0f, }, - .heat_pwm_config={ - .channel=TIM_CHANNEL_1, - .htim=&htim1, + .heat_pwm_config = { + .channel = TIM_CHANNEL_1, + .htim = &htim1, }, - .cali_mode=BMI088_CALIBRATE_ONLINE_MODE, - .work_mode=BMI088_BLOCK_PERIODIC_MODE, + .cali_mode = BMI088_CALIBRATE_ONLINE_MODE, + .work_mode = BMI088_BLOCK_PERIODIC_MODE, }; - // imu=BMI088Register(&imu_config); + // imu=BMI088Register(&imu_config); gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { @@ -65,26 +66,26 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 8, //8 + .Kp = 8, // 8 .Ki = 0, .Kd = 0, .DeadBand = 0.1, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { - .Kp = 50,//40 + .Kp = 50, // 40 .Ki = 200, .Kd = 0, - .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 3000, .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2], + .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, @@ -102,24 +103,24 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp =10,//10 + .Kp = 10, // 10 .Ki = 0, .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, - .IntegralLimit =100, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { - .Kp=50,//50 - .Ki =350,//350 - .Kd =0,//0.1 - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, - .IntegralLimit =2500, + .Kp = 50, // 50 + .Ki = 350, // 350 + .Kd = 0, // 0.1 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 2500, .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]), + .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index dfe0e70..2a871ea 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -113,7 +113,6 @@ typedef struct // config parameter float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|