电磁 源代码

943294069

留下来，发挥点余热，仅作参考

#include <hidef.h>      /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include "stdio.h"
#pragma  LINK_INFO DERIVATIVE "mc9s12xs128"
#define  Cha_zhi_M   17
#define  AD_deadline 20
#define  Mid         3510
#define  speed_MAX   600
#define  speed_MIN   0            
#define  sum_adv     180  
              
uint    ADval0[10],ADval1[10],ADval2[10],ADval3[10],ADval4[10],ADval5[10],ADval6[10],ADval7[10];  
uint    ADvv0,ADvv1,ADvv2,ADvv3,ADvv4,ADvv5,ADvv6,ADvv7;
uint    ADv0,ADv1,ADv2,ADv3,ADv4,ADv5,ADv6,ADv7;
uint    AD_mix[8],AD_Mux_Num0,AD_Mux_Num1,AD_Mux_Num2,ADNum_sum;
float   adnum1,adnum2,adsum,adcha;
float   x=0,last_x,b,angle_DAT,pre_angle=3295,pulse_counter;
uchar   x_work=0,M_flag=0,z_flag=0,zero=1,Break;
float   KP=3,KI=0.02,KD=0.5,D_kp=2,D_kd=0,kk=0.35,bili;//PID的初始值
int     fasong,max_speed,min_speed;
int     stopcount,first=0;
int     de=180,del=20;
int     ERROR1,PWM_OUT;
int     wending_x=0,ruwan=0,leter_x,wdjiasu=0,shache;
long    tiaobian,wandaojs=13000,wdganraojs,wendingganraojs;
long    zhidao_js=0,zdjiasu=0,zdganrao_js,zdwendingganrao_js;
long    wandao_js=13000,wdganrao_js,wendingganrao_js;  
long    chujie_js,cjganrao_js,chujieganrao_js;   
long    s_count=0;  
typedef struct PID           //定义数法核心数据
{
    signed int ideal_speed;  //速度PID，速度设定值
    signed int v_FeedBack;   //速度PID，速度反馈值
    signed long v_PreError;  //速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
    signed long v_PreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror;
    float v_Kp;              //速度PID，Ka = Kp
    float v_Ki;              //速度PID，Kb = Kp * ( T / Ti )
    float v_Kd;              //速度PID，
    signed long Moto_Out;    //电机控制输出值
}
PID;
PID sPID;
/***函数声明***/
void   PLLInit24M(void);
void   SciInit(void);  
void   PWM_Init(void);
void   ATD_Init(void);
void   TimerAccInit(void);
void   GetVal(void);
float  ads(float y);
void   Car_position(void);
void   DJ_angle(void) ;//舵机的角度计算
void   speedset(void);                                                                                                
void   PID_Init(void);
float  PID_Control(PID *pp );
void   GO(void);
void   ifstop(void);
void   stop();
void   if_Break(void);
void   j_line(void);
void   SciWrite();
void   setPID(void);
void main(void)
{
    PLLInit24M();
    while(de--)    //起始延时
    {
      while(del--);
    }
    PWM_Init();
    ATD_Init();
    TimerAccInit();
    PID_Init();
    SciInit();
    speedset();
    EnableInterrupts;
  
    for(;;)
    {
        _FEED_COP();      //看门狗
        GetVal();
        Car_position();
        j_line();         //路况判断
        DJ_angle();
        if_Break();       //刹车
        GO();
        ifstop();         //停车
        if(s_count>2200)
        stop();
    /*    fasong=ADvv0;
      SciWrite(fasong);
      
      fasong=ADvv1;
      SciWrite(fasong);
      
      fasong=ADvv2;         
      SciWrite(fasong);
      
      fasong=ADvv3;
      SciWrite(fasong);
      
      fasong=ADvv4;
      SciWrite(fasong);
      
      fasong=ADvv5;
      SciWrite(fasong);
      fasong=ADvv6;
      SciWrite(fasong);
      fasong=ADvv7;
      SciWrite(fasong);   
      fasong=0x0;
      SciWrite(fasong); */
    }
  
}
//--------------锁相环-------------//
void  PLLInit24M()
{
    CLKSEL&=0x7f;
    PLLCTL_PLLON=0;
    SYNR =0xC5;
    REFDV=0x81;
    PLLCTL_PLLON=1;
    while (( CRGFLG&0x08) == 0x00);
    CLKSEL |= (1<<7);
}
//----------------SCI--------------//
void SciInit()
{
    SCI0BD=0x9c;         //9600bps  Baud Rate=BusClock/(16*SCIBD)
    SCI0CR1=0;         //正常8位模式，无奇偶校验
    SCI0CR2=0X0c;      //发送允许   中断允许
}
//-----------------写SCI数据---------------------------------//
void SciWrite(int fasong)
{
    while (!(SCI0SR1&0x80));
    SCI0DRL=fasong;           
}
//-----------------PWM--------------------------//
void  PWM_Init()
{
    PWMCTL=0xf0;  //01级联，23级联
    PWMPOL=0x00;  //起始输出高电平
    PWMCAE=0x00;  //左对齐输出
    PWMPRCLK=0x12;//01  Clock A=12M，Clock B=24M
    PWMCLK=0xff;  //所有的都选成SA或者SB
    PWMSCLA=0x03; //SA=2MHZ
    PWMSCLB=0x01; //SB=12MHZ
    PWMPER01=6666;//  周期为舵机  ,01级联使用SA时钟源1/((1/2M）*6666)=300HZ
    PWMPER23=600; // 47020 k电机  ,23级联使用SB时钟源1/((1/12M）*600)=20KHZ
    PWMPER67=600; //470600      
    PWMCNT01=0;
    PWMCNT23=0;
    PWMCNT67=0;
    PWME=0xff;
    PWMDTY01=Mid;
    PWMDTY23=0;
    PWMDTY67=0;
}
//-------------------AD-----------------------------//
void ATD_Init(void)
{
    ATD0CTL1 = 0x00;  
    ATD0CTL2 = 0x40; //转换完成后标志位自动清零
    ATD0CTL3 = 0x80; //转换序列长度为8
    ATD0CTL4 = 0x6D; //06
    ATD0CTL5 = 0x30;
    ATD0DIEN = 0x00;
}
//------------------编码器中断时间------------------//
void TimerAccInit(void)
{
    TCTL3 = 0x80;    //CH7 capture on falling edges only
    PACTL_PAEN = 1;
    PACNT = 0;      
    RTICTL = 0xBF;   // 1 100 1111   1/(16M/320*10^3)s = 20ms     
    CRGINT_RTIE = 1;   
}
//-----------------AD读取------------------//
void GetVal()
{
    uchar  i,j,k,m;
    uint  JiaoHuan;
    ADv0=0;ADv1=0;ADv2=0;ADv3=0;ADv4=0;ADv5=0;ADv6=0;ADv7=0;
    for(i=0;i<10;i++)
    {
        while(!ATD0STAT0_SCF);
        ADval0[i]= ATD0DR0L;ADval1[i]= ATD0DR1L;ADval2[i]= ATD0DR2L;ADval3[i]= ATD0DR3L;
        ADval4[i]= ATD0DR4L;ADval5[i]= ATD0DR5L;ADval6[i]= ATD0DR6L;ADval7[i]= ATD0DR7L;        
    }
    for(m=0;m<10;m++)      
    {
        ADv0+=ADval0[m];ADv1+=ADval1[m];ADv2+=ADval2[m];ADv3+=ADval3[m];
        ADv4+=ADval4[m];ADv5+=ADval5[m];ADv6+=ADval6[m];ADv7+=ADval7[m];
    }
        
        ADvv0=ADv0*0.1179;
        ADvv1=ADv1*0.1250;        
        ADvv2=ADv2*0.1250;        
        ADvv3=ADv3*0.0988;
        ADvv4=ADv4*0.1000;        
        ADvv5=ADv5*0.0910;
        ADvv6=ADv6*0.1231;        
        ADvv7=ADv7*0.1221;  
      
        if(ADvv0>AD_deadline||ADvv1>AD_deadline||ADvv2>AD_deadline||ADvv3>AD_deadline||ADvv4>AD_deadline||ADvv5>AD_deadline||ADvv6>AD_deadline||ADvv7>AD_deadline)
             x_work=1;
        else x_work=0;
        if(ADvv0<=65&&ADvv1<=15&&ADvv2<=15&&ADvv3<=15&&ADvv4<=15&&ADvv5<=15&&ADvv6<=15&&ADvv7<=15)
            zero=0;
        else zero=1;
        AD_mix[0]= ADvv0*10+1;
        AD_mix[1]= ADvv1*10+2;
        AD_mix[2]= ADvv2*10+3;
        AD_mix[3]= ADvv3*10+4;
        AD_mix[4]= ADvv4*10+5;
        AD_mix[5]= ADvv5*10+6;
        AD_mix[6]= ADvv6*10+7;
        AD_mix[7]= ADvv7*10+8;
        for(k=0;k<=7;k++)
        {
          for(j=k+1;j<=7;j++)
          {
             if(AD_mix[k]<AD_mix[j])
             {
                 JiaoHuan=AD_mix[k];
                 AD_mix[k]=AD_mix[j];
                 AD_mix[j]=JiaoHuan;
             }
          }
        }
        AD_Mux_Num0 = AD_mix[0]%10;
        AD_Mux_Num1 = AD_mix[1]%10;
        AD_Mux_Num2 = AD_mix[2]%10;
        adnum1=AD_mix[0]/10;
        adnum2=AD_mix[1]/10;
        adsum=adnum1+adnum2;
        bili=sum_adv/adsum;
        adnum1*=bili;
        adnum2*=bili;
        if((AD_mix[0]/10-AD_mix[1]/10)==0)
           M_flag=2;
        if((AD_mix[1]/10-AD_mix[2]/10)==0)
           M_flag=1;      
        ADNum_sum = AD_Mux_Num0 + AD_Mux_Num1;
        adcha=adnum1-adnum2;
}
//----------------绝对值-------------------//
float ads(float y)
{
    if(y<0)  y=-y;
    return y;
}
//---------------偏差计算------------------//
void  Car_position()
{  
    if(x_work&&!(M_flag==1)&&!(M_flag==2))
    {
        switch(ADNum_sum)
        {
          case 3://1，2号传感器的值最大
                 if(AD_Mux_Num0==1)
                   x=-(adcha*30/Cha_zhi_M+90); //m1差值是固定
                 else  if(AD_Mux_Num0==2)
                   x=-(90-adcha*30/4/Cha_zhi_M);
                 b=x;
                 z_flag=0;
                 break;
          case 5:
                 if(AD_Mux_Num0==2)
                   x=-(adcha/2*30/Cha_zhi_M+60);
                 else  if(AD_Mux_Num0==3)
                   x=-(60-adcha/2*30/Cha_zhi_M); //确定车的位置
                 b=x;
                 z_flag=0;
                 break;
          case 7:
                 if(AD_Mux_Num0==3)
                   x=-(adcha/2*30/Cha_zhi_M+30);
                 else   if(AD_Mux_Num0==4)
                   x=-(30-adcha/2*30/Cha_zhi_M);
                 b=x;
                 z_flag=1;
                 break;
          case 9:
                 if(AD_Mux_Num0==4)
                   x=-(adcha/2*30/Cha_zhi_M);
                 else  if(AD_Mux_Num0==5)
                   x=adcha/2*30/Cha_zhi_M;
                 b=x;
                 z_flag=1;
                 break;
          case 11:
                 if(AD_Mux_Num0==5)
                   x=(30-(adcha)/2*30/Cha_zhi_M);
                 else  if(AD_Mux_Num0==6)
                   x=(adcha/2*30/Cha_zhi_M+30);
                 b=x;
                 z_flag=1;
                 break;
          case 13:
                 if(AD_Mux_Num0==6)
                   x=(60-adcha/2*30/Cha_zhi_M);
                 else  if(AD_Mux_Num0==7)
                   x=(adcha/2*30/Cha_zhi_M+60);
                 b=x;
                 z_flag=0;
                 break;
          case 15:
                 if(AD_Mux_Num0==7)
                   x=(90-adcha*30/3/Cha_zhi_M);
                 else  if(AD_Mux_Num0==8)
                   x=(adcha*30/Cha_zhi_M+90);        
                 b=x;
                 z_flag=0;
                 break;
         default:break;
         }
   }
   else   
   {
       x=b;
       M_flag=0;
   }
   tiaobian=ads(x-leter_x);                 //偏差跳变计算
   leter_x=x;
}

13862773798

谢！！！

HAWX

太感谢了

张路军

好东西！楼主好人！

精忠报国

受领了。。

zhangwenqiang

MARK

涛涛

  哈哈哈~~~

涛涛

谢谢楼主！！

f43

谢谢楼主啊

sincelh

呃  看一下