色哟哟视频在线观看-色哟哟视频在线-色哟哟欧美15最新在线-色哟哟免费在线观看-国产l精品国产亚洲区在线观看-国产l精品国产亚洲区久久

電源穩(wěn)壓降壓用LMT317 作為LDO芯片，采用直流無刷電機實現(xiàn)高轉(zhuǎn)速保證動量輪高動量，內(nèi)部集成TB6612驅(qū)動芯片和SY8120IABC穩(wěn)壓芯片保證動量輪處于正常控制狀態(tài)。狀態(tài)反饋傳感器采用JY61陀螺儀。主控芯片采用CH32V103R8T6。

/*說明：片內(nèi)外設(shè)使用了定時器和串口，定時器用來正交解碼讀取編碼器的值，也用于pwm輸出。串口1用來調(diào)試和調(diào)用finSH組件，串口2接收陀螺儀數(shù)據(jù)
內(nèi)核使用了線程和信號量以及中斷*/


//發(fā)送到VOFA+
void sendVofa(float data_1,float data_2)
{
    unsigned char tail[4]={0x00, 0x00, 0x80, 0x7f};
       float fdata[2];
       fdata[0]=data_1;
       fdata[1]=data_2;
       uart_putbuff(UART_1,(char*)fdata,sizeof(float)*2);
       uart_putbuff(UART_1,(char*)tail,4);
}


//數(shù)據(jù)濾波
#define dt 0.012  //這個單位是s，是采樣周期
/**********************************************
* 卡爾曼濾波
***********************************************/
float KalmanFilter_Elect(float curr_elect_val,float last_elect_val)
{
  static float Q_curr = 1.0;//0.1           //Q增大，動態(tài)響應(yīng)增大，過程噪聲的協(xié)方差
  static float Q_last = 0.0001;         //過程噪聲的協(xié)方差，過程噪聲的協(xié)方差為一個一行兩列矩陣
  static float R_elect = 10.0;                  //測量噪聲的協(xié)方差 即測量偏差

  static float Pk[2][2] = { {1, 0}, {0, 1 }};

  static float Pdot[4] = {0,0,0,0};

  static float q_bias = 0.0;
  static float elect_err = 0.0;
  static float PCt_0 = 0.0;
  static float PCt_1 = 0.0;
  static float E = 0.0;
  static float K_0 = 0.0, K_1 = 0.0, t_0 = 0.0, t_1 = 0.0;

  Pdot[0] = Q_curr - Pk[0][1] - Pk[1][0];       //Pk-先驗估計誤差協(xié)方差的微分
  Pdot[1] = -Pk[1][1];
  Pdot[2] = -Pk[1][1];
  Pdot[3] = Q_last;

  Pk[0][0] += Pdot[0] * dt;             //Pk-先驗估計誤差的協(xié)方差微分的積分
  Pk[0][1] += Pdot[1] * dt;             //先驗估計誤差協(xié)方差
  Pk[1][0] += Pdot[2] * dt;
  Pk[1][1] += Pdot[3] * dt;

  elect_err = curr_elect_val - last_elect_val;          //偏差 = 測量值 - 預(yù)測值，先驗估計

  PCt_0 = Pk[0][0];
  PCt_1 = Pk[1][0];

  E = R_elect + PCt_0;

  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;

  t_0 = PCt_0;
  t_1 = Pk[0][1];

  Pk[0][0] -= K_0 * t_0;                    //后驗估計誤差協(xié)方差
  Pk[0][1] -= K_0 * t_1;
  Pk[1][0] -= K_1 * t_0;
  Pk[1][1] -= K_1 * t_1;

  curr_elect_val += K_0 * elect_err;                //后驗估計 更新最優(yōu)電磁值 最優(yōu)電磁值 = 預(yù)測值 + 卡爾曼增益*(測量值-預(yù)測值)
  q_bias += K_1 * elect_err;                //后驗估計 更新誤差

  return curr_elect_val;

}


//控制算法部分

static void calculatepwm(float actualang,float expectang,int32_t actualsped,int32_t expectsped)//控制算法運行
{
            //直立環(huán)經(jīng)典pid+fuzzy：pd
            errbala.err=expectang-actualang;//當(dāng)前誤差
            errbala.derr=gyro;//當(dāng)次誤差與上一次誤差之間的差值
            errbala.err=KalmanFilter_Elect(errbala.err,errbala.lasterr);//卡爾曼濾波
            errbala.derr=KalmanFilter_Elect(errbala.derr,errbala.lastderr);//卡爾曼濾波
            errbala.lasterr=errbala.err;
            errbala.lastderr=errbala.derr;
            errbala.out=kp*errbala.err+kd*errbala.derr;//算出結(jié)果

            //速度環(huán)pi
            errsped.err=expectsped-actualsped;//當(dāng)前誤差
            errsped.sum+=errsped.err;//誤差積分
            if(errsped.sum>1000)//積分限幅
                errsped.sum=1000;
            else if(errsped.sum<-300)
                errsped.sum=-300;
            errsped.out=kp1*errsped.err+ki1*errsped.sum;//算出結(jié)果

            pwmout=errbala.out+errsped.out;//
            if(pwmout>=9999)pwmout=9999;
            else if(pwmout<-9999)pwmout=-9999;


            //這里做最后的輸出控制
            if(pwmout>0)
            {
                pwm_duty(PWM2_CH2_A1, pwmout);
                pwm_duty(PWM3_CH1_A6,9000);
            }
            else
            {
                pwm_duty(PWM2_CH2_A1, -pwmout);
                pwm_duty(PWM3_CH1_A6,0);
            }
            //rt_kprintf("pwmout：%d\r\n",(int32_t)pwmout);
}

//編碼器值獲取及濾波
encoder =(int32_t)(0.7* timer_quad_get(TIMER_4)+0.3*lastenc);//一階低通濾波去毛刺


//陀螺儀數(shù)據(jù)獲取
if (ucRxBuffer[0]!=0x55) //數(shù)據(jù)頭不對，則重新開始尋找0x55數(shù)據(jù)頭
    {
        ucRxCnt=0;
        return;
    }
    if (ucRxCnt<11) {return;}//數(shù)據(jù)不滿11個，則返回
    else
    {
        if(ucRxBuffer[1]==0x53)
        {
        int i=-1;
        while((i++)<11)
        {
            ANGLE[i]=ucRxBuffer[i];
        }
        }
        else if(ucRxBuffer[1]==0x52)
        {
            int i=-1;
            while((i++)<11)
        {
                GYRO[i]=ucRxBuffer[i];
        }
        }

        ucRxCnt=0;//清空緩存區(qū)

    }
    
    roll=((short)(ANGLE[3]<<8|ANGLE[2]))/32768.0*180;//得到角度
        gyro=((short)(GYRO[3]<<8|GYRO[2]))/32768.0*2000;//得到角速度

代碼和3D模型見附件