文章目录

 

---

前言

本文针对 stabilizerInit(); 进行学习

/*电机传感器PID初始化*/

---

一、stabilizerInit(); /*姿态 电机 初始化*/

void stabilizerInit(void) { if(isInit) return; stateControlInit(); /*×Ë̬PID³õʼ»¯*/ powerControlInit(); /*µç»ú³õʼ»¯*/ isInit = true; }

二、stateControlInit();//姿态PID初始化

void stateControlInit(void) {     attitudeControlInit(RATE_PID_DT, ANGEL_PID_DT); /*初始化姿态PID  两个参数分别为1/500 和1/250   */      positionControlInit(VELOCITY_PID_DT, POSITION_PID_DT); /*初始化位置PID 两个参数都为 1/250  */ }

 attitudeControlInit(RATE_PID_DT, ANGEL_PID_DT)

void attitudeControlInit(float ratePidDt, float anglePidDt) { pidInit(&pidAngleRoll, 0, configParam.pidAngle.roll, ratePidDt); /*roll 角度PID初始化*/ pidInit(&pidAnglePitch, 0, configParam.pidAngle.pitch, ratePidDt); /*pitch 角度PID初始化*/ pidInit(&pidAngleYaw, 0, configParam.pidAngle.yaw, ratePidDt); /*yaw 角度PID初始化*/ pidSetIntegralLimit(&pidAngleRoll, PID_ANGLE_ROLL_INTEGRATION_LIMIT); /*roll 角度积分限幅设置*/ pidSetIntegralLimit(&pidAnglePitch, PID_ANGLE_PITCH_INTEGRATION_LIMIT); /*pitch 角度积分限幅设置*/ pidSetIntegralLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT); /*yaw 角度积分限幅设置*/ pidInit(&pidRateRoll, 0, configParam.pidRate.roll, anglePidDt); /*roll 角速度PID初始化*/ pidInit(&pidRatePitch, 0, configParam.pidRate.pitch, anglePidDt); /*pitch 角度PID初始化*/ pidInit(&pidRateYaw, 0, configParam.pidRate.yaw, anglePidDt); /*yaw 角度PID初始化*/ pidSetIntegralLimit(&pidRateRoll, PID_RATE_ROLL_INTEGRATION_LIMIT); /*roll 角速度积分限幅设置*/ pidSetIntegralLimit(&pidRatePitch, PID_RATE_PITCH_INTEGRATION_LIMIT); /*pitch角速度积分限幅设置*/ pidSetIntegralLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT); /*yaw 角速度积分限幅设置*/ }

typedef struct {     float desired;        //< set point     float error;        //< error     float prevError;    //< previous error     float integ;        //< integral  偏差对时间的积分     float deriv;        //< derivative   偏差对时间的微分     float kp;           //< proportional gain     float ki;           //< integral gain     float kd;           //< derivative gain     float outP;         //< proportional output (debugging)     float outI;         //< integral output (debugging)     float outD;         //< derivative output (debugging)     float iLimit;       //< integral limit     float outputLimit;  //< total PID output limit, absolute value. '0' means no limit.     float dt;           //< delta-time dt } PidObject;

PidObject pidAngleRoll; PidObject pidAnglePitch; PidObject pidAngleYaw; PidObject pidRateRoll; PidObject pidRatePitch; PidObject pidRateYaw;

pidInit() 和pidSetIntegralLimit()  就是对各个PID结构体进行初始化 赋参数

参数来源于之前的一个    configParamInit();    /*初始化配置参数*/   里面根据版本 初始化了参数。

三、powerControlInit();//电机初始化

void powerControlInit(void) {     motorsInit(); } //拒绝俄罗斯套娃 为什么不直接写 motorsInit();

 

void motorsInit(void) /*µç»ú³õʼ»¯*/ { //定义了GPIO初始化结构体 计时器初始化结构体 PWM通道设置结构体 GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; //使能GPIO总线时钟 使能端口定时器复用时钟 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB, ENABLE); //ʹÄÜPORTA PORTBʱÖÓ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM4,ENABLE); //TIM2ºÍTIM4ʱÖÓʹÄÜ //初始化定时器TIM2 TIM4为默认状态 TIM_DeInit(TIM4); // TIM_DeInit(TIM2); // GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4); //PB7复用为TIM4 CH2 MOTOR1 GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4); //PB6复用为TIM4 CH1 MOTOR2 GPIO_PinAFConfig(GPIOB,GPIO_PinSource10,GPIO_AF_TIM2); //PB10复用为TIM2 CH3 MOTOR3 GPIO_PinAFConfig(GPIOA,GPIO_PinSource5,GPIO_AF_TIM2); //PA5 复用为TIM2 CH1 MOTOR4 //根据复用功能 设置GPIO引脚 并初始化 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_10; //PB6 7 10 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; //¸´Óù¦ÄÜ GPIO_InitStructure.GPIO_Speed=GPIO_Speed_100MHz; //ËÙ¶È100MHz GPIO_InitStructure.GPIO_OType=GPIO_OType_PP; //ÍÆÍ츴ÓÃÊä³ö GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP; //ÉÏÀ­ GPIO_Init(GPIOB,&GPIO_InitStructure); //³õʼ»¯PB6 7 10 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5; //PA5 GPIO_Init(GPIOA,&GPIO_InitStructure); //³õʼ»¯PA5 //设置并初始化计时器 TIM_TimeBaseStructure.TIM_Period=MOTORS_PWM_PERIOD; //×Ô¶¯ÖØ×°ÔØÖµ TIM_TimeBaseStructure.TIM_Prescaler=MOTORS_PWM_PRESCALE; //¶¨Ê±Æ÷·ÖƵ TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //ÏòÉϼÆÊýģʽ TIM_TimeBaseStructure.TIM_ClockDivision=0; //ʱÖÓ·ÖƵ TIM_TimeBaseStructure.TIM_RepetitionCounter=0; //Öظ´¼ÆÊý´ÎÊý TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure); //³õʼ»¯TIM4 TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStructure); //³õʼ»¯TIM2 //设置并初始化计时器的PWM输出 TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1; //PWMģʽ1 TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable; //ʹÄÜÊä³ö TIM_OCInitStructure.TIM_Pulse=0; //CCRx TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_High; //¸ßµçƽÓÐЧ TIM_OCInitStructure.TIM_OCIdleState=TIM_OCIdleState_Set; //¿ÕÏиߵçƽ TIM_OC2Init(TIM4, &TIM_OCInitStructure); //³õʼ»¯TIM4 CH2Êä³ö±È½Ï TIM_OC1Init(TIM4, &TIM_OCInitStructure); //³õʼ»¯TIM4 CH1Êä³ö±È½Ï TIM_OC3Init(TIM2, &TIM_OCInitStructure); //³õʼ»¯TIM2 CH3Êä³ö±È½Ï TIM_OC1Init(TIM2, &TIM_OCInitStructure); //³õʼ»¯TIM2 CH1Êä³ö±È½Ï TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); //ʹÄÜTIM4ÔÚCCR2ÉϵÄԤװÔؼĴæÆ÷ TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); //ʹÄÜTIM4ÔÚCCR1ÉϵÄԤװÔؼĴæÆ÷ TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); //ʹÄÜTIM2ÔÚCCR3ÉϵÄԤװÔؼĴæÆ÷ TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); //ʹÄÜTIM2ÔÚCCR1ÉϵÄԤװÔؼĴæÆ÷ TIM_ARRPreloadConfig(TIM4,ENABLE); //TIM4 ARPEʹÄÜ TIM_ARRPreloadConfig(TIM2,ENABLE); //TIM2 ARPEʹÄÜ TIM_Cmd(TIM4, ENABLE); //ʹÄÜTIM4 TIM_Cmd(TIM2, ENABLE); //ʹÄÜTIM2 isInit = true; }

使能GPIO  使能复用时钟

设置定时器默认模式

设置并初始化GPIO

设置并初始化定时器

设置并初始化PWM输出

---

总结

stabilizerInit(); 里面包含了以下两个函数

stateControlInit();//姿态PID初始化

powerControlInit();//电机初始化