智能车制作

标题: 这是128调直立的程序，启动后进不了中断，麻烦大家帮忙看看 [打印本页]

作者: 远点水 时间: 2014-4-5 20:43
标题: 这是128调直立的程序，启动后进不了中断，麻烦大家帮忙看看
#include <hidef.h>    /* common defines and macros */
#include "derivative.h"    /* derivative-specific definitions */
#include <MC9S12XS128.h>
#include <stdio.h>
#include <math.h>

/*PLL INITIAL*/
void SetBusCLK_80M(void) //利用PLL将16MHz晶振频率转为80MHz
{
CLKSEL_PLLSEL=0; //OR：CLKSEL=0x00 不使能锁相环时钟，使用外部晶振
PLLCTL_PLLON=1;  //锁相环电路允许
SYNR=0xC0|0x09;  //VCO_clock=2*osc_clock*(1+SYNR)/(1+REFDV)=160MHz
                  //VCOFRQ[1:0]=1:1,代表VCO_clock在80~120MHz之间（160M超频）
REFDV=0x80|0x01; //REF_clock=osc_clock/(REFDV+1)=16/(1+1)=8
                  //因为参考时钟在6~12MHz之间，所以REFFRQ[1:0]=1:0
POSTDIV=0x00;  //VCO_clock=PLL_clock,BUS_clock=PLL_clock/2；即BUS_clock=80MHz
_asm(nop); //短暂暂时，等待时钟频率稳定
_asm(nop);
while(!(CRGFLG_LOCK==1)); //等待时钟频率稳定，锁相环频率锁定
CLKSEL_PLLSEL=1; //使能锁相环时钟，BUS采用根据PLL频率设定
}

/*ATD INITIAL*/
void ATD_Init(void)
{
  ATD0DIEN=0x00; //禁止数字输入功能
  ATD0CTL1=0x50; //分辨率12位数据,采样前放电
  ATD0CTL2=0x40; //快速清除序列修改所有ATD转换完成标志位，禁止外部触发，禁止中断
  ATD0CTL3=0xC0; //转换结果右对齐，每个序列8个通道，非FIFO模式
  ATD0CTL4=0x13; //采样时间4个周期，PRS=19，ATD_clock=80/2/PRS=2M(500k~2M为AD模块时钟允许频率)
  ATD0CTL5=0x30; //从通道0开始多通道连续采样，同时启动ATD转换序列
}

/*PWM INITIAL*/
void PWM_Init(void)    //输出频率15KHz
{
  PWME=0x00;    //禁止PWM模块
  PWMPRCLK=0x33;  //CLOCKA、B都为8分频,80MHz/8=1=10MHz
  PWMSCLA=0x01; //比例因子为1，SA时钟二分频，10MHz/(1*2)=5MHz
  PWMSCLB=0x01; //比例因子为1，SB时钟二分频，10MHz/(1*2)=5MHz
  PWMPOL=0xAA; //PWM输出先为高电平，后为低电平

  PWMCTL_CON01=1; //01通道级联使用
  PWMCLK_PCLK1=1; //选择SA作为时钟源
  PWMCAE_CAE1=1; //居中对齐输出
  PWMPER01=200;    //通道1周期寄存器设置为200
  PWMDTY01=0;    //占空比寄存器设置
  PWMCNT01=0x00; //级联后计数器清零
  PWME_PWME1=1;    //使能01通道

  PWMCTL_CON23=1; //23通道级联使用
  PWMCLK_PCLK3=1; //选择SB作为时钟源
  PWMCAE_CAE3=1; //居中对齐输出
  PWMPER23=200;    //通道3周期寄存器设置为200
  PWMDTY23=0;    //占空比寄存器设置
  PWMCNT23=0x00; //级联后计数器清零
  PWME_PWME3=1;    //使能23通道

  PWMCTL_CON45=1; //45通道级联使用
  PWMCLK_PCLK5=1; //选择SA作为时钟源
  PWMCAE_CAE5=1; //居中对齐输出
  PWMPER45=200;    //通道5周期寄存器设置为200
  PWMDTY45=0;    //占空比寄存器设置
  PWMCNT45=0x00; //级联后计数器清零
  PWME_PWME5=1;    //使能45通道

  PWMCTL_CON67=1; //67通道级联使用
  PWMCLK_PCLK7=1; //选择SB作为时钟源
  PWMCAE_CAE7=1; //居中对齐输出
  PWMPER67=200;    //通道7周期寄存器设置为200
  PWMDTY67=0;    //占空比寄存器设置
  PWMCNT67=0x00; //级联后计数器清零
  PWME_PWME7=1;    //使能67通道
}

/* 陀螺仪、加速度计数据获取 */
float AD0[20],AD1[20],AD2[20],ADvalue0,ADvalue1,ADvalue2;
unsigned char a;
void get_tuoluo_acc(void)
{
  for(a=0;a<20;a++)
  {
while(!ATD0STAT0_SCF); //判断转换序列是否完成，完成则往下执行
AD0[a]=ATD0DR0;
AD1[a]=ATD0DR1;
AD2[a]=ATD0DR2;
  }
  ADvalue0=(AD0[0]+AD0[1]+AD0[2]+AD0[3]+AD0[4]+AD0[5]+AD0[6]+AD0[7]+AD0[8]+AD0[9]+AD0[10]+AD0[11]+AD0[12]+AD0[13]+AD0[14]+AD0[15]+AD0[16]+AD0[17]+AD0[18]+AD0[19])/20;
  ADvalue1=(AD1[0]+AD1[1]+AD1[2]+AD1[3]+AD1[4]+AD1[5]+AD1[6]+AD1[7]+AD1[8]+AD1[9]+AD1[10]+AD1[11]+AD1[12]+AD1[13]+AD1[14]+AD1[15]+AD1[16]+AD1[17]+AD1[18]+AD1[19])/20;
  ADvalue2=(AD2[0]+AD2[1]+AD2[2]+AD2[3]+AD2[4]+AD2[5]+AD2[6]+AD2[7]+AD2[8]+AD2[9]+AD2[10]+AD2[11]+AD2[12]+AD2[13]+AD2[14]+AD2[15]+AD2[16]+AD2[17]+AD2[18]+AD2[19])/20;
}

/* 互补滤波函数（陀螺仪&加速度计） */
volatile float Acc; //加速度
volatile float Gyr; //陀螺仪
volatile float Gyr_jiao; //陀螺仪的角速度
volatile float Acc_jiao; //加速度的角度
volatile float real_angle;

/*
void  hubulvbo(void)
{
  float Q=0.99,R=0.01;  //陀螺仪&加速度计权重

  if (ADvalue2<1270) //加速度过冲保护 1940恰好直立 2635  1270
   ADvalue2=1270;
  if (ADvalue2>2635)
   ADvalue0=2635;

  Acc=ADvalue2-1940;    //加速度计的值

  Gyr=2380-ADvalue0;    //陀螺仪的值

  Acc_jiao=Acc*0.132;    //z轴转过的角度 180°/(2635-1270)=0.132°

  Gyr_jiao=Gyr*0.36;    //陀螺仪的角度    5/4096/0.67/5*1000
  real_angle=Q*(real_angle+Gyr_jiao*0.05)+R*(Acc_jiao); //互补滤波,0.01采样周期
} */
volatile float AngleIntegral,jiaodu_cha;
void AngleCalculate(void)
{
  float fDeltaValue;
  Acc=ADvalue2-2000;    //加速度计的值
  Acc_jiao=Acc*0.132;    //z轴转过的角度 180°/(2635-1270)=0.132°
  Gyr=ADvalue0-2245;    //陀螺仪的值
  Gyr_jiao=Gyr*0.36;    //陀螺仪的角度    5/4096/0.67/5*1000
  real_angle=AngleIntegral;
  fDeltaValue=(Acc_jiao-real_angle)/4;
  AngleIntegral+=(Gyr_jiao+fDeltaValue)/200;
}

/* 平衡控制 */
volatile float P_angle,D_angle,speedout_angle;

void angle_control(void)
{
  P_angle=0.1;                      //角度P
  D_angle=0;                      //角度D
  speedout_angle=AngleIntegral*P_angle+Gyr_jiao*D_angle;
}

void IO_Init(void)
{
  DDRB=0xCF;
  PORTB_PB2=1;
  PORTB_PB3=0;
}

void delay(void)
{
  int i,j;
  for(i=0;i<5000;i++)
for(j=0;j<5000;j++);
}

/*PIT INITIAL(1ms)*/
void PIT_Init(void)    //定时周期=（PITMTLD0+1)*(PITLD0+1)/(80M)=(7+1)*(9999+1)/(80M)=1ms
{
  PITCFLMT=0x00; //禁止PIT模块
  PITCE_PCE0=1;    //使能定时器通道0
  PITMUX=0x00;    //定时器通道0使用微定时基准0计数
  PITMTLD0=0x07; //设置8位微定时装载寄存器0初值为7
  PITLD0=9999;    //设置16位装载寄存器0初值为9999
  PITINTE=0x01;    //使能PIT定时器通道0中断
  PITCFLMT=0x80; //使能PIT模块
}

int num;

#pragma CODE_SEG __NEAR_SEG NON_BANKED
interrupt VectorNumber_Vpit0 void Pit0Interrupt(void)
{
  if(num==1)
  {
get_tuoluo_acc();
//hubulvbo();
AngleCalculate();
  }
  if(num==4)
  {
angle_control();
if(speedout_angle<=0)
{
   PWMDTY01=0;
   PWMDTY23=(int)(15-speedout_angle);
   PWMDTY45=(int)(15-speedout_angle);
   PWMDTY67=0;
}
else
{
   PWMDTY01=(int)(15+speedout_angle);
   PWMDTY23=0;
   PWMDTY45=0;
   PWMDTY67=(int)(15+speedout_angle);
}
num=0;
  }
  num++;
  PITTF_PTF0=1;       //清除中断标志
}

#pragma CODE_SEG DEFAULT

void main(void)
{
  DisableInterrupts;
  SetBusCLK_80M();
  ATD_Init();
  PWM_Init();
  IO_Init();
  PIT_Init();
  delay();
  EnableInterrupts;

  for(;;)
  { }
}

作者: 远点水 时间: 2014-4-14 01:34
解决方案：用到了IO口的中断复用引脚，如当做IO口用要将对应端口的中断标志禁用！

作者: jamesZB 时间: 2014-4-16 19:27
ADvalue1的值是什么啊？为什么没有用啊？

作者: ★奔/1孓亥★ 时间: 2014-5-29 22:31
O(∩_∩)O谢谢

作者: 大眼睛 时间: 2014-7-1 11:09
加个QQ545008241 请教问题

作者: 远点水 时间: 2014-7-1 13:43

大眼睛发表于 2014-7-1 11:09
加个QQ545008241 请教问题

可以在这里问呀！

作者: 大眼睛 时间: 2014-7-2 10:51

远点水发表于 2014-7-1 13:43
可以在这里问呀！

车子做好了直立不会调！！！

作者: 远点水 时间: 2014-7-2 11:07

大眼睛发表于 2014-7-2 10:51
车子做好了直立不会调！！！

你参考一下第七届电磁直立的官方方案，是完全可以做好的。对了，你是用什么芯片？

作者: 远点水 时间: 2014-7-2 11:08

大眼睛发表于 2014-7-2 10:51
车子做好了直立不会调！！！

调直立要用上位机看波形的跟随性

作者: 大眼睛 时间: 2014-7-2 12:59

远点水发表于 2014-7-2 11:07
你参考一下第七届电磁直立的官方方案，是完全可以做好的。对了，你是用什么芯片？

XS128呀

作者: 远点水 时间: 2014-7-3 14:00

大眼睛发表于 2014-7-2 12:59
XS128呀

XS128直立没有问题，如果加上摄像头就要好好处理摄像头的时序，因为128的资源比较有限。

欢迎光临智能车制作 (http://dns.znczz.com/)