智能车制作

标题: 急~~~~~~请教高手帮我解释一下这个程序，是磁导 4个传感器的程序 [打印本页]

作者: 周嘉 时间: 2011-4-22 10:22
标题: 急~~~~~~请教高手帮我解释一下这个程序，是磁导 4个传感器的程序
请前辈详细解释，因为我确实读不懂这程序，我是初学者，正参加学习的本科生选拔赛，谢谢了哈~~~~~~~~~~~~~~~~~·这是我的邮箱305121385@qq.com 可以发我邮箱！感激不尽
#include <hidef.h>    /* common defines and macros */
#include "derivative.h"    /* derivative-specific definitions */

#include <math.h>
#define motor_pwm_frequ  6  //khz //电机PWM频率
#define bus_clk 80    //mhz    //单片机运行频率
#define  servo_period 20 //4ms       //舵机周期
#define RIGHT_SERVO_R 410//530    //右极限          1230
#define RIGHT_SERVO_L 410//530//670//(-670) //左极限          -1250
  unsigned int  RIGHT_SERVO_MID=3725;
unsigned char if_ctl_speed;
unsigned char setted_speed;
int cur_speed;

void Dly_ms(unsigned int ms)
{
unsigned int ii,jj;
if (ms<1) ms=1;
for(ii=0;ii<ms;ii++)
   //for(jj=0;jj<3338*2;jj++); //80MHz--1ms

  for(jj=0;jj<2000;jj++);
}

void SetBusCLK_80M(void)
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x09;
REFDV=0x80 | 0x01;
POSTDIV=0x00;    //pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz;
_asm(nop);       //BUS CLOCK=80M
_asm(nop);
while(!(CRGFLG_LOCK==1));    //when pll is steady ,then use it;
CLKSEL_PLLSEL =1;          //engage PLL to system;
}
void vinterruptsinit(void)
{
TIOS=0x00; //外部输入捕捉0,1通道 0 输入捕捉 1 输出比较7
TCTL4=0b10100000;  //全部下降沿有效//0表示ICx禁止, 1表示上升沿, 2表示下降沿, 3表示任何沿

TSCR1=0x80;    //定时器控制寄存器1使能

}

//////stl测速定时器中断/////////////
void Init_INT_Timer(void)
{
TSCR2_PR=0;
TSCR2_TOI|=1;
TSCR1_TEN=1;
}

//*******************************读取转换结果******************************
byte ReadATD(byte ch)
{
  byte ad=0;
  while(!ATD0STAT0_SCF);
  switch(ch)
  {
default:
case 0:
   ad= ATD0DR0L;
break;
case 1:
   ad= ATD0DR1L;
break;
case 2:
   ad= ATD0DR2L;
break;
case 3:
   ad= ATD0DR3L;
break;
case 4:
   ad= ATD0DR4L;
break;
case 5:
   ad= ATD0DR5L;
break;
case 6:
   ad= ATD0DR6L;
break;
case 7:
   ad= ATD0DR7L;
break;
  }
  return ad;
}

/***************************IO口初始化************************************/
void vIOPortInit(void)
{

DDRA=0X00;    //input
DDRB=0xff;    //output

DDRS_DDRS2=1;
DDRS_DDRS3=1;
DDRH=0xff;
DDRJ_DDRJ6=1;
DDRJ_DDRJ7=1;

////////////////////////ad///////////////////////////////////

ATD0DIEN=0x00;    //禁止数字输入
ATD0CTL1=0x00;
ATD0CTL2=0b10000000; //0b11000000; //
ATD0CTL3=0b11000000; //A/D转换序列长度为8
ATD0CTL4=0b00010110; //8位精度，ATDclock=Busclock/(PRS+1)/2=BusClock/40=2MHz,要求ATDclock<=2MHz&&ATDclock>=500KHz
//ATD0CTL4=0x81; // 8位精度, 2个时钟, ATDClock=[BusClock*0.5]/[PRS+1] ; PRS=1, divider=12

ATD0CTL5=0b00110000; //右对齐无符号数,连续转换序列模式,2通道采样，AN0-AN7模拟量通道输入
///////////////////////ad/////////////////////////////////////

}

/****SCI初始化****/
void  ini_SCI(void){
SCI0BD=(bus_clk*1000000/115200/16); //115200=(busclk*1000000/SCI0BD/16);
SCI0CR1=0x00;
SCI0CR2=0x0C;
}
/****测速初始化****/
void speed_Init(void) {

//pacn32和pacn1,pacn0可用
{

  PACTL_PAEN=1;  //16位脉冲累加器A使能

  TCTL4_EDG1x=3;//上升下降沿都捕捉
  TCTL4_EDG0x=3;


}

}

/** PWM初始化***/
void Init_PWMout(void)
{
PWME = 0x00;  //禁止输出
PWMPOL = 0xff;//极性寄存器，1 先输出高电平后为低电平 0 先输出低电平后为高电平
PWMCTL = 0xf0; //01 23 45 67 通道级联
PWMPRCLK=0x55; //Clock A=Bus Clock/32  ; Clock B=Bus Clock/32;
PWMCLK=0b00000000; //选择clock:7B,6B,5A,4A,3B,2B,1A,0A
PWME = 0xaa;    //pp1,pp3,pp5,pp7口输出

PWMPER67 = bus_clk/4*125/motor_pwm_frequ;
PWMPER45 = bus_clk/4*125/motor_pwm_frequ;
PWMPER23 = bus_clk/4*servo_period*125;       //舵机3
PWMPER01 = bus_clk/4*125/motor_pwm_frequ;    //电机1
PWMDTY67 =0;
PWMDTY45 =0;
PWMDTY23 =0;
PWMDTY01 =0;

PTS_PTS2=0;
PTS_PTS3=0;

}

/******************************/
void PWMoutVec(unsigned int Velocity) {

if(Velocity>PWMPER01) Velocity=PWMPER01- 50;

PWMDTY01 = PWMPER01-Velocity; /* 设置电机速度 */
PWMDTY45 = PWMPER45;  /* 设置电机速度 */

}

void PWMoutStopVec(unsigned int Velocity) {

PWMDTY01=PWMPER01;
PWMDTY45=PWMPER45;
return;
}

void PWMoutReverseVec(int Velocity) {

PWMDTY01=PWMPER01;
PWMDTY45=PWMPER45;
return;
}
void pwm_setright(unsigned int dutycycle){
if(dutycycle<RIGHT_SERVO_MID-RIGHT_SERVO_L)dutycycle=RIGHT_SERVO_MID-RIGHT_SERVO_L;
if(dutycycle>RIGHT_SERVO_MID+RIGHT_SERVO_R)dutycycle=RIGHT_SERVO_MID+RIGHT_SERVO_R;
PWMDTY23 = dutycycle;//+65; /* 设置舵机角度 */
}
unsigned char g_IfCross;
unsigned int  wait_cross_pic_num;
unsigned char g_cross_num;
/*速度控制部分*/
unsigned int Speed_cnt,Speed_set;  //测速脉冲个数,设定车速
signed int Speed_d=0,Speed_d_last=0;  //定义设定车速及最大、最小车速

void control_speed() {
signed int tmp_speed=0;
unsigned char doub;
doub=1;

if(if_ctl_speed==0x55)
{

//setted_speed*=5;
//PWMoutVec(setted_speed);
//return;

Speed_d_last=Speed_d;
  Speed_d=setted_speed-cur_speed;//(PA0H-PA1H);
if(Speed_d>=3*doub){    //全油
  PWMoutVec(1000);

  }
  else if(Speed_d<-5*doub){  //全刹
PWMoutReverseVec((0-Speed_d-5*doub)*5);
  }
  else if(Speed_d<=-4*doub){  //速度较快,给一点油,惯性减速
PWMoutVec(1);
  }
  else if(Speed_d<3*doub&&Speed_d>-4*doub){

  if(setted_speed>0) {

//速度基本可以,用PID
tmp_speed=0xA0+Speed_d*60+(Speed_d-Speed_d_last)*2;
if(tmp_speed<0)
   PWMoutStopVec(0-tmp_speed);
else
   PWMoutVec(tmp_speed);
  }
  else
   PWMoutStopVec(1000);
  }}}

unsigned char maxspeed,minspeed;
unsigned int counter_ccw;
/************************************主函数************************************/
void main(void)
{unsigned int i;

unsigned char AD_wData[8];  //全局变量存放 AD结果
unsigned char j;
unsigned char k;
unsigned char Started;

unsigned char miny,minx;
//unsigned char Stopped;
unsigned char max_v[8]={0,0,0,0,0,0,0,0};
const unsigned char min_v[8]={0,0,0,0,0,0,0,0};

const float dif_x[4]=//{-11.8,-4,4,11.8};
            {-11.8,-4,4,12};
//const unsigned char ad_define[4]={0,2,4,6};
const unsigned char ad_define[4]={0,1,2,3};

float sumxy1,sumy1;
float Kp,Kd;
float setted_speed_n;
float dif_old,dif;

Kp=DisableInterrupts;

SetBusCLK_80M();
Init_INT_Timer();
vinterruptsinit();

vIOPortInit();
ini_SCI();
speed_Init();
Init_PWMout();

PORTB_PB0=1;

EnableInterrupts;

TIE_C3I = 0;
TIE_C2I = 0;
TIE_C1I = 0;
TIE_C0I = 1;

PWMDTY01=PWMPER01;
PWMDTY45=PWMPER45;
maxspeed=10;
  minspeed=2;
  pwm_setright(RIGHT_SERVO_MID);

Started=0;
///////////////////归一化/////////////////////////

wait_cross_pic_num=0;
g_cross_num=0;
g_IfCross=0;

dif_old=0;
//Stopped=0;
if_ctl_speed=0x55;
setted_speed=50;

  for(j=0;j<8;j++)AD_wData[j]=ReadATD(j);
//printp("请开始摇头\n");
PORTB=0x55;

for(i=0;i<8000*2;i++)
{

  for(j=0;j<8;j++)
{
AD_wData[j]=ReadATD(j);
//if(AD_wData[j]<min_v[j]) min_v[j]= AD_wData[j];
//else
if(AD_wData[j]>max_v[j]) max_v[j]= AD_wData[j];
}

Dly_ms(2);

}

///////////////////归一化/////////////////////////
{

sumxy1=0;
//  sumxy2=0;
sumy1=0;
// sumy2=0;
miny=0;
minx=0;

for(i=0;i<4;i++)
{

  AD_wData[i]=ReadATD(ad_define[i]);
  if(AD_wData[i]>=min_v[ad_define[i]])
  {
    AD_wData[i]=(AD_wData[i]-min_v[ad_define[i]])*100/(max_v[ad_define[i]]-min_v[ad_define[i]]);
  }

  else
AD_wData[i]=0;

if(AD_wData[i]>miny)
{

  miny=AD_wData[i];
  minx=i;
}

//printp("%03d,",AD_wData[i]);

sumxy1+=dif_x[i]*AD_wData[i];
sumy1+=AD_wData[i];
}

if(Started==0)
{
for( k=0;k<4;k++)
{
PORTB_PB0=0;
Dly_ms(2000);
PORTB_PB0=1;
Dly_ms(2000);
}

Started=1;

}

if(Started==1)
{



  if(sumy1>0)

  dif=(sumxy1/sumy1)*0.6;
else
{
  if(dif_old>0)
dif=8.5;
  else
dif=-8.5;
}

Kp=52;//GetKp(abs(sumxy/sumy));//105;
Kd=0;//30;
if(miny>40) {
setted_speed_n=maxspeed-((dif)*(dif)*(maxspeed-minspeed))/(64.);
if(setted_speed_n<minspeed)setted_speed_n=minspeed;
if(setted_speed_n>maxspeed)setted_speed_n=maxspeed;

setted_speed=setted_speed_n;
control_speed();

if(sumxy1/sumy1>(8.5)) {

pwm_setright(RIGHT_SERVO_MID-RIGHT_SERVO_L);
dif_old=sumxy1/sumy1;
}
else if(sumxy1/sumy1<(-8.5)) {
pwm_setright(RIGHT_SERVO_MID+RIGHT_SERVO_R);
dif_old=sumxy1/sumy1;
}
else

{
pwm_setright(RIGHT_SERVO_MID-dif*Kp);
dif_old=sumxy1/sumy1;

   }
}
else
{

  if((dif_old)>2)
pwm_setright(RIGHT_SERVO_MID-RIGHT_SERVO_L);
  else if((dif_old)<-2)
pwm_setright(RIGHT_SERVO_MID+RIGHT_SERVO_R);
else if(sumxy1/sumy1>0)
pwm_setright(RIGHT_SERVO_MID-RIGHT_SERVO_L);
else
pwm_setright(RIGHT_SERVO_MID+RIGHT_SERVO_R);

setted_speed=minspeed;
control_speed();

}}}
}

/*
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 11 Port0_interrupt(void) //pt3
{

g_IfCross=1;

TFLG1_C3F=1; //清除行中断标志
}

#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 10 Port1_interrupt(void) //pt2
{

g_IfCross=1;
//PORTB=0x00;
TFLG1_C2F=1;

}
*/
/*
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 9 Port2_interrupt(void) //pt1
{

counter_cw++;
//PORTB=0x00;
TFLG1_C1F=1;

}
*/
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 8 Port3_interrupt(void) //pt0
{

counter_ccw++;
//PORTB=0x00;
TFLG1_C0F=1;

}

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 16 INT_Timer0(void)
{

cur_speed=counter_ccw;
counter_ccw=0;
//counter_cw=0;

TFLG2_TOF=1;
}

作者: 等你转身123 时间: 2011-5-2 22:55
慢慢学呗，都不是很会。

作者: shenzhigang 时间: 2011-5-21 11:42
摇头。。？

作者: 清水闲人 时间: 2012-3-21 19:13

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