自平衡小车

a4516987

l两三天的功夫搞定一个小车  不过前进还没弄好~~~

http://v.youku.com/v_show/id_XNTI3MTkwODAw.html

a4516987

void Kalman(float *Z,int length)
{

        unsigned char const n = 2/*,m = 0,k = 0*/;

        unsigned char i = 0;          //计数

        static        const        float         A[n] = {0,0},
                                                        H[n] = {0,0};

        static        const        char        I[n] = {0,0};

           static         const        float         Q[n] = {0.01,0.01},                //过程产生的高斯噪声，和
                                                        R[n] = {0.01,0.01};                  //测量产生的高斯噪声，在这里假设为常值。

        static         float        COV_P[n]         = {0,0},

                                        COV_A[n]        = {0,0};                 //前者是先验估计，后者是后验估计值也是最优估计值。都是协方差

        static        float         X_P[n]                 = {0,0},                  //类同上，不过这次是数值 也就是求解

                                        X_A[n]                 = {0,0};

        static         float        Kg[n]                 = {0,0};                //卡尔曼的增益

        for(i = 0; i < n; i++)
        {
                //时间更新方程。
                COV_P[i] = A[i] * COV_A[i] * A[i] + Q[i];                                         //<1>卡尔曼第一个公式：对于协方差的预估。(这里后面那个应该是转置的因为用FOR了所以不用了)
               
                X_P[i] = A[i] * X_A[i];                                                                                 //<2>卡尔曼第一个公式：对于数值的预估。
               
                //状态更新方程。                                                                                                         
                Kg[i] = (COV_P[i] * H[i]) / (H[i] * COV_P[i] * H[i] + R[i]);//<3>求解卡尔曼增益
               
                X_A[i] = X_P[i] + Kg[i] * (Z[i] - H[i] * X_P[i]);                        //<4>求解最优解，用来回归循环。
               
                COV_A[i] = (I[i] - Kg[i]) * COV_P[i];                                                //<5>求解最优协方差,即最小协方差使之下一个预测数值准确。
        }

a4516987

/*----------------------------------------------------------------------------*/
/*
        File Name                        :        ADXL345.C

    Description                        :

    Author                                :          Jones.Lee
        Copyright                        :         Jones_Workspace
    Version&Date                :         <2012.8.26>&<V1.0>
    Connector Fuction         :
    Notes        ：
*/
/*----------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------*/

    /*---Head Files---*/

        #include "ADXL345.h"

        #include "uart.h"

        #include "math.h"
    /*---Variable Definition--*/

        int         Gravity_X = 0;       
        int         Gravity_Y = 0;       
        int                Gravity_Z = 0;
        int                Gravity[3] = { 0, 0, 0};

        float        Angle_X = 0;
        float        Angle_Y = 0;
        float        Angle_Z = 0;
        float        Angle[3] = { 0, 0, 0};
    /*---Function Definiton---*/

        unsigned char         ReadID(void);

        void                         ADXL345_Init(void);

        void                         Read_Accelerate(unsigned int * pwGyroX, unsigned int * pwGyroY, unsigned int * pwGyroZ);

        void                         Read_Accelerate_Filter(unsigned int * pwGyroX,unsigned int * pwGyroY, unsigned int * pwGyroZ);

        void                          Read_Angle(float * pwAngleX,float * pwAngleY,float * pwAngleZ);

        unsigned char         Checksun(char * byData, unsigned char byLen);

        unsigned char        ADXL345_Read_Byte( unsigned char byAD,unsigned short int byRA);

        void                         ADXL345_Write_Byte( unsigned char byAD,unsigned short int byRA,unsigned char DataToWrite);

        void                         ADXL345_Read_Bytes(unsigned char byAD, unsigned char byRA,unsigned char * pData, unsigned char byCount);

        void                         ADXL345_Write_Bytes(unsigned char byAD, unsigned char byRA,unsigned char * pData, unsigned char byCount);
    /*---Macro Definition-----*/

        #define IIC_ADDR    0x53                  //ADXL345的地址
       
        #define IIC_WR      0xA6                  //写地址
       
        #define IIC_RE      0xA7                  //读地址

        #define         GREATH_DATA                          3.9                   //  2/0xff
        #define         DEG_RAD                                57.32          //就是180/ 3.14

        #define                FLAG_RAD_DEG                1
/*----------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------*/
/*
Fuction Name      ：          ADXL345_Init

Description       ：        

Input parameter   ：         Class                        Name                Action       
                                                 void
Output parameter  ：        Class                        Name                Action
                                                void
Author&Date       ：        Jones.Lee & 2012.6.28

Notes             ：
*/
/*----------------------------------------------------------------------------*/
void ADXL345_Init(void)
{

    unsigned char MSub;
    unsigned char byData[13];

    //数据通信格式;设置为自检功能禁用,4线制SPI接口,低电平中断输出,13位全分辨率,输出数据右对齐,16g量程

    MSub = 0x31;

    byData[0] = 0x2B;
    ADXL345_Write_Bytes(IIC_WR, MSub, byData, 1);

    MSub = 0x1E;

    byData[ 0] = 0x00;                 //X轴误差补偿; (15.6mg/LSB)
    byData[ 1] = 0x00;                 //Y轴误差补偿; (15.6mg/LSB)
    byData[ 2] = 0x00;          //Z轴误差补偿; (15.6mg/LSB)

    byData[ 3] = 0x00;          //DUR                        敲击延时0:禁用;                                 (1.25ms/LSB)
    byData[ 4] = 0x00;          //Latent                检测第一次敲击后的延时0:禁用;         (1.25ms/LSB)
    byData[ 5] = 0x00;                 //Window                敲击窗口0:禁用;                                 (1.25ms/LSB)
    byData[ 6] = 0x01;          //THREST_Act        保存检测活动阀值;                         (62.5mg/LSB)
    byData[ 7] = 0x01;          //THREST_INACT        保存检测静止阀值;                         (62.5mg/LSB)
    byData[ 8] = 0x2B;          //TIME_INACT        检测活动时间阀值;                         (    1s/LSB)
    byData[ 9] = 0x00;                //ACT_INACT_CTL        交流直流转换位
    byData[10] = 0x09;                 //THREST_FF                自由落体检测推荐阀值; (62.5mg/LSB)
    byData[11] = 0xFF;          //TIME_FF                自由落体检测时间阀值,设置为最大时间; (5ms/LSB)
    byData[12] = 0x80;
    ADXL345_Write_Bytes(IIC_WR, MSub, byData, 13);

    MSub = 0x2C;

    byData[ 0] = 0x0A;                //BW_RATE
    byData[ 1] = 0x28;          //POWER_CTL                开启Link,测量功能;关闭自动休眠,休眠,唤醒功能
    byData[ 2] = 0x00;          //INT_ENABLE        所有均关闭
    byData[ 3] = 0x00;          //INT_MAP                中断功能设定,不使用中断
        byData[ 4] = 0x00;                //INT_SOURS
    ADXL345_Write_Bytes(IIC_WR, MSub, byData, 4);

    //FIFO模式设定,Stream模式，触发连接INT1,31级样本缓冲
//        MSub = 0x31;
//        byData[ 0] = 0x0B;           //16G
//    ADXL345_Write_Bytes(IIC_WR, MSub, byData, 1);

    MSub = 0x38;

    byData[ 0] = 0x9F;           //FIFO_CTL             读写
    ADXL345_Write_Bytes(IIC_WR, MSub, byData, 1);

}

/*----------------------------------------------------------------------------*/
/*
Fuction Name      ：          Read_Accelerate

Description       ：        

Input parameter   ：         Class                        Name                Action       
                                                 unsigned int*         pwGyroX       
                                                unsigned int*        pwGyroY
                                                unsigned int*        pwGyroZ
Output parameter  ：        Class                        Name                Action
                                                void
Author&Date       ：        Jones.Lee & 2012.10.5

Notes             ：
*/
/*----------------------------------------------------------------------------*/
void Read_Accelerate(unsigned int * pwGyroX,unsigned int * pwGyroY, unsigned int * pwGyroZ)
{
    unsigned char MSub;
    unsigned char byData[6];
    unsigned int wTemp;
    MSub = 0x32;
   
    ADXL345_Read_Bytes(IIC_WR, MSub, byData, 6);
    wTemp  = 0;
    wTemp = byData[1] << 8;
    wTemp |= byData[0];
    *pwGyroX = wTemp;

    wTemp  = 0;
    wTemp = byData[3] << 8;
    wTemp |= byData[2];
    *pwGyroY = wTemp;
   
    wTemp  = 0;
    wTemp = byData[5] << 8;
    wTemp |= byData[4];
    *pwGyroZ = wTemp;
}
// 接收十次数据 然后截尾均值，防止出现意外
void Read_Accelerate_Filter(unsigned int * pwGyroX,unsigned int * pwGyroY, unsigned int * pwGyroZ)
{
        uint32 wGyroX[10],wGyroY[10],wGyroZ[10];
        uint8 i;
        for(i = 0;i < 10; i++ )
        {
                Read_Accelerate(&wGyroX[i],&wGyroY[i],&wGyroZ[i]);       
        }
                //* pwGyroX =Trimmean(wGyroX,10);
                //* pwGyroY =Trimmean(wGyroY,10);
                //* pwGyroX =Trimmean(wGyroX,10);

}

void  Read_Angle(float * pwAngleX,float * pwAngleY,float * pwAngleZ)
{
        unsigned char i;

//        unsigned char cnt;
//
//        static           unsigned  int err_mid[3];       

        unsigned int mid[3];

           short int mids[3];

        double midf[3];

        unsigned int greath;

        unsigned short int m;

        greath = greath;                                          //消除Warning

        m = m;

        Read_Accelerate(mid,mid+1,mid+2);

        for(i = 0; i < 3; i++)
        {
                mids[i] = mid[i];

                midf[i]        = mids[i];

                midf[i]        = ((midf[i]*GREATH_DATA));                   // GREATH_DATA表示 3.9mg/LSB
        }
               
#if  0                                                                                        //这种算法很简单，但是不好。有误差

        greath = sqrt(pow(midf[0],2)+pow(midf[1],2)+pow(midf[2],2));

        *pwAngleX =asin(midf[0]/greath);                           //x轴的偏角

        *pwAngleY =asin(midf[1]/greath);

        *pwAngleZ =asin(midf[2]/greath);
         
#else                                                                          //这种算法很复杂，建立模型，但是很简单，我套用公式，也懂了

        *pwAngleX = atan( midf[0] / ( sqrt( pow( midf[1] , 2 ) + pow( midf[2] ,2 ))));//x轴的偏角

        *pwAngleY = atan( midf[1] / ( sqrt( pow( midf[0] , 2 ) + pow( midf[2] , 2))));

        *pwAngleZ = atan( ( sqrt( pow( midf[0] , 2 ) + pow( midf[1] , 2))) / midf[2]);
#endif

#if  FLAG_RAD_DEG                                                //转化为角度。

        *pwAngleX *=DEG_RAD;  

        *pwAngleY *=DEG_RAD;

        *pwAngleZ *=DEG_RAD;
#endif

#if  0                                                                        //检验 在运行 清零

        if(*pwAngleX<0)
        {
                  *pwAngleX *=-1;
                UART_Send_Byte('s');

        }

          UART_Send_Byte('x');
        m = *pwAngleX;
        INT_ASC(m);


        if(*pwAngleY<0)
        {
                  *pwAngleY *=-1;
                UART_Send_Byte('s');
        }
        UART_Send_Byte('y');
        m = *pwAngleY;
        INT_ASC(m);



        if(*pwAngleZ<0)
        {
                  *pwAngleZ *=-1;
                UART_Send_Byte('s');

        }
        UART_Send_Byte('z');
        m = *pwAngleZ;
        INT_ASC(m);
                  
        UART_Send_Byte(0x0D);
        UART_Send_Byte(0x0A);
#endif

}
/*----------------------------------------------------------------------------*/
/*
Fuction Name      ：          Checksun

Description       ：        

Input parameter   ：         Class                        Name                Action       
                                                 char *                         byData       
                                                unsigned char        byLen
Output parameter  ：        Class                        Name                Action
                                                unsigned char
Author&Date       ：        Jones.Lee & 2012.10.5

Notes             ：
*/
/*----------------------------------------------------------------------------*/
unsigned char Checksun(char * byData, unsigned char byLen)
{
    unsigned char i;

        char byChecksum;

    byChecksum = 0;

    for (i = 0; i < byLen; i++)
    {
        byChecksum += byData[i];
    }

    return byChecksum;
}



void ADXL345_Write_Byte(uint8 byAD,uint16 byRA, uint8 DataToWrite)
{               
          
    I2C_Send_Ctrl(byAD);                 //发送ADXL地址         
         
        I2C_Send_Byte(byRA);                 //发送地址        
                                                                                                    
        I2C_Send_Byte(DataToWrite);     //发送字节
                                                                                                 
    I2C_Stop();//产生一个停止条件

        delay_ms(3);// 这个延时绝对不能去掉         

}

uint8 ADXL345_Read_Byte(uint8 byAD,uint16 byRA)
{                                  
        unsigned char temp=0;
                                                                                                                                                                       
        I2C_Send_Ctrl(byAD);                            //先呼叫从机为写模式

    I2C_Send_Byte(byRA);                        //发送写地址   

        I2C_Send_Ctrl(byAD+1);                        //转为读模式  
         
    temp=I2C_Recieve_Byte();
                  
    I2C_Stop();                                                //产生一个停止条件
                    
        return temp;
}

void ADXL345_Read_Bytes(unsigned char byAD, unsigned char byRA,unsigned char * pData, unsigned char byCount)
{
        while(byCount)
        {
                *pData++=ADXL345_Read_Byte(byAD,byRA++);       

                byCount--;
        }
}  

void ADXL345_Write_Bytes(unsigned char byAD, unsigned char byRA,unsigned char * pData, unsigned char byCount)
{
        while(byCount--)                                                                        
        {

                ADXL345_Write_Byte(byAD,byRA,*pData);

                byRA++;

                pData++;
        }
}

a4516987

没有编码器

leexy

哈哈  很欢乐啊
视频最后一句耍赖皮，解释了所有~~~
楼主加油啊

a4516987

leexy 发表于 2013-3-15 17:52
哈哈  很欢乐啊
视频最后一句耍赖皮，解释了所有~~~
楼主加油啊

哈哈  学弟扯淡啊  哈哈。不过前进还是问题，

wcm_e

也蛮不错了

a4516987

wcm_e 发表于 2013-3-15 18:47
也蛮不错了

必须的，编码器快来啦，哈哈 我就弄了几天 我觉得很好啦。

sedulity11

mark，关注下

破破破东风

楼主很强大！

a4516987

破破破东风 发表于 2013-3-16 08:50
楼主很强大！

低调~~哈哈

a4516987

里面的程序有三轴的加速度倾角和陀螺仪 是为了四周飞控。

945595199

楼主你好，我感觉你立的非常的好！
请问你用什么电机是什么型号啊？能给个电机的资料吗？

linpeixing

好稳..........

a4516987

linpeixing 发表于 2014-12-23 20:36
好稳..........

以前做的， 现在做的很稳啦，做的三环控制，加前馈 做了小飞机~~

linpeixing

a4516987 发表于 2014-12-24 18:58
以前做的， 现在做的很稳啦，做的三环控制，加前馈 做了小飞机~~

你前馈是怎么做的？