智能车制作

标题: 请问各位老师及大吓们 [打印本页]

作者: 明风二号 时间: 2012-5-3 18:25
标题: 请问各位老师及大吓们
我的车子，调了一个月了，但是还不能成功立起来。
用的是官网的方案，角度输出一直调得不好
输出范围总在5度左右的范围内波动

附一个截图，垂直方向范围是80到90度（是车子静止时候的），
陀螺仪的输出波动很小，加速度器波动范围较大

小弟请问大侠们有什么建议
下周就校内赛了，心急~~~~

作者: 明风二号 时间: 2012-5-3 18:47
按照官网的做法是：时间补偿常数取2到5之间的，但是我这样取是得不出正确的角度值的，只有取0.005这样才能够~~~

作者: Alliswall 时间: 2012-5-3 19:40
上面是加速度的波形？

作者: xiexueshi 时间: 2012-5-3 20:55

明风二号发表于 2012-5-3 18:47
按照官网的做法是：时间补偿常数取2到5之间的，但是我这样取是得不出正确的角度值的，只有取0.005这样才能够 ...

时间常数调到1~4候在调Rgyro,你的串口发送正常吗，模块正常吗，最好拿别人的模块试试你的程序

作者: kidbang 时间: 2012-5-3 21:12
上面的波形是什么？

作者: kidbang 时间: 2012-5-3 21:12
融合后的波形应该很平滑那

作者: 明风二号 时间: 2012-5-3 22:57
上面是融合后角度输出的波形~~~~~

作者: 明风二号 时间: 2012-5-3 22:58

xiexueshi 发表于 2012-5-3 20:55
时间常数调到1~4候在调Rgyro,你的串口发送正常吗，模块正常吗，最好拿别人的模块试试你的程序

用CW看的

作者: 明风二号 时间: 2012-5-3 22:58

kidbang 发表于 2012-5-3 21:12
融合后的波形应该很平滑那

不平滑啊，有很大的波动

作者: 明风二号 时间: 2012-5-4 00:10
附个代码出来，哪位高手可以指点下，我实在搞不懂了~~·
#include <hidef.h>    /* common defines and macros */
#include "derivative.h"    /* derivative-specific definitions */
#define Tg  0.004       //同样的算法，但是跟官网的差别很大（时间补偿常数）？？
#define  jsdq_ratio 0.57
#define  jiaodu_speed_ratio 1.1
#define jadq_offset  290
#define  jiaodu_speed_offset 340
float jsdq=0.0,jadq_a=0.0,jiaosu_speed=0.0,jiaodu0=0.0,jiaodu=0.0,jiaosu_speed_a=0.0;

//bus clock
void SetBusCLK_nM(byte nM)
{
MMCCTL1=0X00;
PKGCR  =0X06;
DIRECT =0x00;
IVBR =0xFF;
ECLKCTL=0xC0;
CPMUPROT =0x26; //停止保护时钟配置寄存器
CPMUCLKS_PSTP=0; //
CPMUCLKS_PLLSEL=1;  //应用PLL
//CPMUSYNR =nM-1; //设置分频因子
CPMUSYNR =0x40|(nM-1); //设置分频因子
CPMUREFDIV =0x80|0x00; //pllclock=2*(1+SYNR)= MHz;
CPMUPOSTDIV=0x00; // Set the post divider register
CPMUPOSTDIV=0x00; // Set the post divider register
CPMUPLL =0x10; // Set the PLL frequency modulation
while(CPMUFLG_LOCK == 0); /* Wait until the PLL is within the desired tolerance of the target frequency */
CPMUPROT=0x00;          /* Enable protection of clock configuration registers */
}
void Dly_ms(int ms)
{
int ii,jj;
if (ms<1) ms=1;
for(ii=0;ii<ms;ii++)
   for(jj=0;jj<2670;jj++); //busclk:16MHz--1ms
}
static void Port_Init(void) //灯低电平亮
{
DDRD = 0xff;  //LED  PTB0--7,
// PORTD= 0xff;  //LEDs off
PORTD= 0xf0;  //LEDs on

}
void AD_Init()//4路，陀螺仪、加速度各一路，两路备用
{ ATDCTL1=0x20;//10位
//ATDCTL1=0x40;  //12位
  //ATDCTL1=0x00; //7:1-外部触发,65:00-8位精度,4:放电,3210:ch
  ATDCTL2=0x40; //禁止外部触发, 中断禁止
  ATDCTL3=0x80; //右对齐无符号,每次转换8个序列, No FIFO, Freeze模式下继续转
  ATDCTL4=0x01; //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]
  ATDCTL5=0x30; //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样
  ATDDIEN=0x00; //禁止数字输入
//Led2=0;//led2

}
void RTI_Init(void)//中断初始化,1ms定时中断
{
  //CPMURTI=0Xff; //3.2M/16M=0.5s
  CPMURTI=0X8F;//7:1,654:000 16k  ,16k/16M=0.001s=1ms
  CPMUINT=0X80;//中断允许
}
void Get_AD_average() {
float acceleration[20];
float gyroscope[20];
int m;
float  acceleration_sum=0,gyroscope_sum=0;
for(m=0;m<20;m++) {
  acceleration[m]= ATDDR1;
  acceleration_sum+=acceleration[m];
  gyroscope[m]=ATDDR0;
  gyroscope_sum+=gyroscope[m] ;
}
jadq_a= acceleration_sum/20;
jiaosu_speed_a=gyroscope_sum/20;
//Led3=0;
}
void Calculate_angle(){
  float xiuzheng=0.0f;
  jsdq=(jadq_a-jadq_offset)*jsdq_ratio;          //jsdq=加速度器 jadq_a为20次采样平均值 _offset是初始值
  jiaosu_speed=(jiaosu_speed_a-jiaodu_speed_offset)*jiaodu_speed_ratio;//jiaodu为陀螺仪的信号
                                                //jiaosu_speed_a:陀螺仪平均值
                                                //jiaodu_speed_offset  初始值
  jiaodu0=jiaodu;
  xiuzheng=(jsdq-jiaodu0)/Tg;
  jiaodu+=(jiaodu0+xiuzheng)/200;
}
void main(void) {
  /* put your own code here */
  SetBusCLK_nM(48) ;
  Port_Init();
  AD_Init();
  RTI_Init();
EnableInterrupts;
  PORTD= 0x0f;
  for(;;) {
_FEED_COP(); /* feeds the dog */
  } /* loop forever */
  /* please make sure that you never leave main */
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 7 RTI(void){
static int i=0;
i++;
CPMUFLG_RTIF=1;//RTI time-out has occurred清中断标志位
if(i==1){
Get_AD_average();//AD采集、滤波
Calculate_angle();
//Angle_Control();
}
else if(i==5){
//MotorOutput();// Motor out
  i=0

ORTD= 0x00;
}

//Led6=0;
}

作者: tongyonghua123 时间: 2012-5-4 13:48

明风二号发表于 2012-5-3 22:58
不平滑啊，有很大的波动

cw怎么看。。。

作者: lixianghua0428 时间: 2012-5-4 19:45
融合后很好的锯齿波，建议LZ看看陀螺仪静动态时的输出是否平滑，否则换个陀螺仪

作者: 明风二号 时间: 2012-5-5 00:17

lixianghua0428 发表于 2012-5-4 19:45
融合后很好的锯齿波，建议LZ看看陀螺仪静动态时的输出是否平滑，否则换个陀螺仪

静态时候很平滑······几乎不动，但是加速度器的静态是波动蛮大的

作者: zhangyechu 时间: 2012-5-5 06:28

明风二号发表于 2012-5-4 00:10
附个代码出来，哪位高手可以指点下，我实在搞不懂了~~·
#include /* common defines and macros */ ...

这个貌似有点问题#define jadq_offset 290
290会不小了点？陀螺仪可能有问题

作者: zhangyechu 时间: 2012-5-5 06:31

zhangyechu 发表于 2012-5-5 06:28
这个貌似有点问题#define jadq_offset 290
290会不小了点？陀螺仪可能有问题

请教一下，楼主用CW怎么看波形的？

作者: 明风二号 时间: 2012-5-5 13:41

zhangyechu 发表于 2012-5-5 06:28
这个貌似有点问题#define jadq_offset 290
290会不小了点？陀螺仪可能有问题

那个是加速度器的，10位AD，转换过来也有1.4V这样，个人觉得没有问题

欢迎光临智能车制作 (http://111.231.132.190/)