自平衡小车 (amobbs.com 阿莫电子技术论坛) -

a4516987 发表于 2013-3-15 17:10:08

自平衡小车

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

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

a4516987 发表于 2013-3-15 17:11:08

void Kalman(float *Z,int length)
{

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

unsigned char i = 0; //计数

static const float A = {0,0},
H = {0,0};

static const char I = {0,0};

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

static float COV_P = {0,0},

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

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

X_A = {0,0};

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

for(i = 0; i < n; i++)
{
//时间更新方程。
COV_P = A * COV_A * A + Q;    //<1>卡尔曼第一个公式：对于协方差的预估。(这里后面那个应该是转置的因为用FOR了所以不用了)

X_P = A * X_A;    //<2>卡尔曼第一个公式：对于数值的预估。

//状态更新方程。
Kg = (COV_P * H) / (H * COV_P * H + R);//<3>求解卡尔曼增益

X_A = X_P + Kg * (Z - H * X_P); //<4>求解最优解，用来回归循环。

COV_A = (I - Kg) * COV_P; //<5>求解最优协方差,即最小协方差使之下一个预测数值准确。
}

a4516987 发表于 2013-3-15 17:11:44

/*----------------------------------------------------------------------------*/
/*
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 = { 0, 0, 0};

float Angle_X = 0;
float Angle_Y = 0;
float Angle_Z = 0;
float Angle = { 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;

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

MSub = 0x31;

byData = 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 = 0x09; //THREST_FF 自由落体检测推荐阀值; (62.5mg/LSB)
byData = 0xFF; //TIME_FF 自由落体检测时间阀值,设置为最大时间; (5ms/LSB)
byData = 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;
unsigned int wTemp;
MSub = 0x32;

ADXL345_Read_Bytes(IIC_WR, MSub, byData, 6);
wTemp= 0;
wTemp = byData << 8;
wTemp |= byData;
*pwGyroX = wTemp;

wTemp= 0;
wTemp = byData << 8;
wTemp |= byData;
*pwGyroY = wTemp;

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

}

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

// unsigned char cnt;
//
// static    unsignedint err_mid;

unsigned int mid;

   short int mids;

double midf;

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 = mid;

midf = mids;

midf = ((midf*GREATH_DATA));    // GREATH_DATA表示 3.9mg/LSB
}

#if0 //这种算法很简单，但是不好。有误差

greath = sqrt(pow(midf,2)+pow(midf,2)+pow(midf,2));

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

*pwAngleY =asin(midf/greath);

*pwAngleZ =asin(midf/greath);

#else       //这种算法很复杂，建立模型，但是很简单，我套用公式，也懂了

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

*pwAngleY = atan( midf / ( sqrt( pow( midf , 2 ) + pow( midf , 2))));

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

#ifFLAG_RAD_DEG //转化为角度。

*pwAngleX *=DEG_RAD;

*pwAngleY *=DEG_RAD;

*pwAngleZ *=DEG_RAD;
#endif

#if0 //检验在运行清零

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;
}

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 发表于 2013-3-15 17:18:26

没有编码器

leexy 发表于 2013-3-15 17:52:21

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

a4516987 发表于 2013-3-15 18:39:05

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

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

wcm_e 发表于 2013-3-15 18:47:17

{:lol:} 也蛮不错了

a4516987 发表于 2013-3-15 23:19:40

wcm_e 发表于 2013-3-15 18:47 static/image/common/back.gif
也蛮不错了

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

sedulity11 发表于 2013-3-16 00:29:43

mark，关注下

破破破东风 发表于 2013-3-16 08:50:05

楼主很强大！

a4516987 发表于 2013-3-16 09:57:18

破破破东风发表于 2013-3-16 08:50 static/image/common/back.gif
楼主很强大！

低调~~哈哈

a4516987 发表于 2013-3-16 09:58:06

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

945595199 发表于 2014-12-23 18:41:31

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

linpeixing 发表于 2014-12-23 20:36:53

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

a4516987 发表于 2014-12-24 18:58:39

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

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

linpeixing 发表于 2014-12-24 22:23:36

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

你前馈是怎么做的？

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

自平衡小车