|
|
#include "Control.h"
|
|
|
#include "tim.h"
|
|
|
#include "inv_mpu.h"
|
|
|
|
|
|
Chassis chassis;
|
|
|
|
|
|
/*********************** λ<>û<EFBFBD> ****************************/
|
|
|
float Pos_Pid_Get = 0;
|
|
|
float Pos_Pid_Set = 0;
|
|
|
float Pos_Pid_KP = 0;
|
|
|
float Pos_Pid_KD = 0;
|
|
|
float Pos_Pid_Error[2] = {0, 0};
|
|
|
float Pos_Pid_Pout, Pos_Pid_Dout, Pos_Pid_Out;
|
|
|
|
|
|
/*********************** <20>ٶȻ<D9B6> ****************************/
|
|
|
float Speed_Pid_Get = 0;
|
|
|
float Speed_Pid_Set = 0;
|
|
|
float Speed_Pid_KP = 0;
|
|
|
float Speed_Pid_KD = 0;
|
|
|
float Speed_Pid_Error[2] = {0, 0};
|
|
|
float Speed_Pid_Pout, Speed_Pid_Dout, Speed_Pid_Out;
|
|
|
|
|
|
// <20>涨OLED<45><44><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>
|
|
|
|
|
|
static void Left_Set_Speed(float PWM) {
|
|
|
|
|
|
// <20><>ת
|
|
|
if (PWM > 0) {
|
|
|
HAL_GPIO_WritePin(GPIOB, AIN1_Pin, GPIO_PIN_SET);
|
|
|
HAL_GPIO_WritePin(GPIOB, AIN2_Pin, GPIO_PIN_RESET);
|
|
|
|
|
|
__HAL_TIM_SET_COMPARE(&PWM_TIMER, PWM_LEFT_CHANNEL, PWM);
|
|
|
} else {
|
|
|
HAL_GPIO_WritePin(GPIOB, AIN1_Pin, GPIO_PIN_RESET);
|
|
|
HAL_GPIO_WritePin(GPIOB, AIN2_Pin, GPIO_PIN_SET);
|
|
|
|
|
|
__HAL_TIM_SET_COMPARE(&PWM_TIMER, PWM_LEFT_CHANNEL, -PWM);
|
|
|
}
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
static void Right_Set_Speed(float PWM) {
|
|
|
|
|
|
// <20><>ת
|
|
|
if (PWM > 0) {
|
|
|
HAL_GPIO_WritePin(GPIOB, BIN1_Pin, GPIO_PIN_RESET);
|
|
|
HAL_GPIO_WritePin(GPIOB, BIN2_Pin, GPIO_PIN_SET);
|
|
|
|
|
|
__HAL_TIM_SET_COMPARE(&PWM_TIMER, PWM_RIGHT_CHANNEL, PWM);
|
|
|
} else {
|
|
|
HAL_GPIO_WritePin(GPIOB, BIN1_Pin, GPIO_PIN_SET);
|
|
|
HAL_GPIO_WritePin(GPIOB, BIN2_Pin, GPIO_PIN_RESET);
|
|
|
|
|
|
__HAL_TIM_SET_COMPARE(&PWM_TIMER, PWM_RIGHT_CHANNEL, -PWM);
|
|
|
}
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
float pitch_temp;
|
|
|
float pitch_gyro_temp;
|
|
|
|
|
|
static void Get_Chassis_Angle(void) {
|
|
|
|
|
|
float yaw_temp, roll_temp;
|
|
|
|
|
|
mpu_dmp_get_data(&pitch_temp, &roll_temp, &yaw_temp, &pitch_gyro_temp);
|
|
|
|
|
|
chassis.pitch = -pitch_temp;
|
|
|
chassis.pitch_gyro = -(pitch_gyro_temp / 16.4f);
|
|
|
}
|
|
|
|
|
|
int16_t count = 0;
|
|
|
|
|
|
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
|
|
|
|
|
|
count++;
|
|
|
|
|
|
Get_Chassis_Angle();//获取当前角度
|
|
|
|
|
|
float gravity_error = 1;//重心误差补偿
|
|
|
|
|
|
//PID双环控制输出结果
|
|
|
float pid_pos = pid_calc(&chassis.chassis_balance_pos_pid,chassis.pitch,gravity_error);
|
|
|
float pid_speed = pid_calc(&chassis.chassis_balance_speed_pid,chassis.pitch_gyro,pid_pos);
|
|
|
// float final_pid_output = pid_pos;
|
|
|
float final_pid_output = pid_speed;
|
|
|
chassis.Motor_Left.output = -final_pid_output;
|
|
|
chassis.Motor_Right.output = -final_pid_output;
|
|
|
|
|
|
//电机限制，防止烧坏电机
|
|
|
if (chassis.Motor_Left.output > 7200) {
|
|
|
chassis.Motor_Left.output = 7200;
|
|
|
} else if (chassis.Motor_Left.output < -7200) {
|
|
|
chassis.Motor_Left.output = -7200;
|
|
|
}
|
|
|
|
|
|
if (chassis.Motor_Right.output > 7200) {
|
|
|
chassis.Motor_Right.output = 7200;
|
|
|
} else if (chassis.Motor_Right.output < -7200) {
|
|
|
chassis.Motor_Right.output = -7200;
|
|
|
}
|
|
|
|
|
|
Left_Set_Speed(chassis.Motor_Left.output);
|
|
|
Right_Set_Speed(chassis.Motor_Right.output);
|
|
|
|
|
|
} |
