电磁 源代码

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

943294069

//------------------道路分析-----------------//
void j_line()
{   
    if(tiaobian<6)                          //稳定赛道
    {
      wandaojs+=pulse_counter;              //累加编码器脉冲
      if((wandaojs-wendingganraojs)>500)    //跳变干扰排除
        wdganraojs=0;
    }
    else
    {                                       //不稳定次数累加
      wdganraojs++;
      wendingganraojs=wandaojs;
      if(wdganraojs>4)
      {
         wandaojs=0;
         wdganraojs=0;
      }
    }

////////////////////////////////////////   
    if(ads(x)<80)               //直道判断
    {
      zhidao_js+=pulse_counter;              //直道脉冲累加
      if((zhidao_js-zdwendingganrao_js)>300)  //直道跳变排除
        zdganrao_js=0;
    }
    else                                    //直道不稳定累加
    {
      zdganrao_js++;
      zdwendingganrao_js=zhidao_js;
      if(zdganrao_js>4)      
      {
        zhidao_js=0;
        zdganrao_js=0;
      }
    }
///////////////////////////////////////////     
    if(ads(x)>=80&&ads(x)<220)
    {   
      wandao_js+=pulse_counter;              //累加编码器脉冲
      if((wandao_js-wendingganrao_js)>300)    //跳变干扰排除
        wdganrao_js=0;  
    }
    else
    {                                       //不稳定次数累加
      wdganrao_js++;
      wendingganrao_js=wandao_js;
      if(wdganrao_js>4)      
      {
        wandao_js=0;
        wdganrao_js=0;
      }
    }
///////////////////////////////////////////
    if(ads(x)>=220)               //直道判断
    {
      chujie_js+=pulse_counter;              //直道脉冲累加
      if((chujie_js-chujieganrao_js)>300)  //直道跳变排除
        cjganrao_js=0;
    }
    else                                    //直道不稳定累加
    {
      cjganrao_js++;
      chujieganrao_js=chujie_js;
      if(cjganrao_js>4)      
      {
        chujie_js=0;
        cjganrao_js=0;
      }
    }     
}
//-----------------刹车-------------------------//
void  if_Break()
{   
    Break=0;                                      //正反转标志
   
    if( (zhidao_js>15000&&ads(x)>=80)||(wandao_js>20000&&(ads(x)>=220||ads(x)<80))||(chujie_js>20000&&ads(x)<65))      
        ruwan=1;
    else ruwan=0;
   
    if(ruwan==1||shache>0)//弯道刹车判断
    {
      PWMDTY67 =600;
      PWMDTY23 =0;
      if(pulse_counter>65)                        //刹车判断
        shache++;
      else  shache=0;
      if(shache==1)                             //计数清零
      {
          ruwan=0;
          wandaojs=0;
          wendingganraojs=0;
          wdganraojs=0;
          zhidao_js=0;zdganrao_js=0;zdwendingganrao_js=0;
          wandao_js=0;wdganrao_js=0;wendingganrao_js=0;
          chujie_js=0;cjganrao_js=0;chujieganrao_js=0;
      }
      Break=1;                                    //刹车标志为1
    }
}
//--------------舵机的角度计算-----------------//
void DJ_angle(void)
{   
    angle_DAT=Mid+(x)*D_kp+(x-last_x)*D_kd;//舵机的角度控制
    pre_angle=angle_DAT;            //存储当前的车的坐标和舵机的拐角，以便下次作为前次的值来使用
    if(angle_DAT>4610)
      angle_DAT=4610;
    else if(angle_DAT<2410)
      angle_DAT=2410;
    last_x=x;
}
//---------------拨码开关--------------------------//
void   speedset()
{
    DDRB=0x00;
    if     (!(PORTB&0x01))
    {  
        D_kp=1.85; D_kd=40;  max_speed=70;
    }
    else if(!(PORTB&0x02))
    {
        D_kp=1.9; D_kd=40;  max_speed=80;
    }
    else if(!(PORTB&0x04))
    {
        D_kp=1.95; D_kd=70;  max_speed=90;
    }
    else if(!(PORTB&0x08))
    {
        D_kp=2.3; D_kd=100;  max_speed=95;
    }
    else if(!(PORTB&0x10))
    {   
        D_kp=2.1; D_kd=65;  max_speed=100;
    }
    else if(!(PORTB&0x20))
    {   
        D_kp=2.1; D_kd=105;  max_speed=130;
    }
    else if(!(PORTB&0x40))
    {   
        D_kp=2.1; D_kd=110;  max_speed=135;
    }
     else if(!(PORTB&0x80))
    {   D_kp=2.1; D_kd=120;  max_speed=140;
        
    }  
}
//-----------------结构体-----------------//
void   PID_Init()
{
    sPID.ideal_speed = 0 ;        //速度设定值，自己定的，会随着程序的执行有不同的变化，这是一个变的值。
    sPID.v_FeedBack = 0 ;  //速度反馈值这是由编码器里测出来的。  
    sPID.v_PreError = 0 ;   //前一次，速度误差,ideal_speed - vi_FeedBack：就是设定速度和真实速度的差
    sPID.v_PreDerror = 0 ;   //前一次，速度误差之差，d_error-PreDerror;设定速度和真实速度的差是所谓的误差，将前一次误差和这一次误差相差就是误差之差
    sPID.v_Kp = KP;
    sPID.v_Ki = KI;
    sPID.v_Kd = KD;            
    sPID.Moto_Out = 0; //电机控制输出值//占空比的设定，初始为0
}
/*-------PID系数设置------*/
void  setPID(void)
{
   
        sPID.v_Kp=5;
        sPID.v_Ki=0.01;
        sPID.v_Kd=0;
   
}
//-----------------电机PID----------------//
float PID_Control(PID *pp )
{  
   signed long  error1,error2,error3;//error是预定的速度和返回速度之差，d_error是偏差的偏差
   
   pp->v_FeedBack= pulse_counter;
  
   if(wandaojs>10000)                           //稳定赛道加速
   {
      if(wdjiasu<21)                            //递增
        wdjiasu+=3;
   }                          
   else
      wdjiasu=0;                                //否则为0
   pp->ideal_speed=max_speed+wdjiasu;
   error1 = (signed long)(pp->ideal_speed - pp->v_FeedBack); // 当前偏差计算   
   ERROR1 = error1;
   error2 = error1 - pp->v_PreError;//速度的偏差的偏差
   error3 = error2 - pp->v_PreDerror;//这一次偏差的差和前一次偏差的差的差
   pp->v_PreError = error1; //存储当前偏差        
   pp->v_PreDerror = error2;//存储偏差的偏差
   setPID();
   
      pp->Moto_Out+=(signed long)((pp ->v_Kp*error2)+pp->v_Ki*error1+ pp->v_Kd*error3);
   
   if( pp->Moto_Out > speed_MAX )
      pp->Moto_Out = speed_MAX;
   if( pp->Moto_Out < speed_MIN )
      pp->Moto_Out = speed_MIN;
   return (pp->Moto_Out);// 返回预调节占空比
}
//------------------执行-----------------//
void GO()
{   
   
   PWMDTY01 = angle_DAT;
    if(Break==0)
    {  
        PWM_OUT  = PID_Control(&sPID);
        PWMDTY23 = PWM_OUT;
        PWMDTY67 = 0;
    }
}
//-----------------停车条件---------------//
void  ifstop()
{
    char j=0;
    DDRA=0x00;
    if(!(PORTA&&0x01))
    {           
        if(stopcount<100)
          first=1;
        else  stop();      
    }   if(first==1)
      stopcount++;
    if(stopcount>1000)
      stopcount=1000;
}
//-------------------停车-----------------//
void   stop()
{
    for(;;)
    {
        GetVal();
        Car_position();
        j_line();
        DJ_angle();
        PWMDTY01=angle_DAT;
        PWMDTY23=0;
        PWMDTY67=6*pulse_counter;
        if(pulse_counter==0)
        while(1)
        {
            PWMDTY23=0;   
            PWMDTY67=0;
        }
    }
  
}

#pragma CODE_SEG NON_BANKED
void interrupt 7 RTI_interrupt(void)//做什么用的
{
    CRGFLG |= 0x60;  
    pulse_counter = PACNT*0.8;//n/360*18/76*16.7,,,,0.8
    PACNT = 0;
    s_count++;
}

aspwen

谢谢，参考共享。想请问一下，你的电磁是怎样排布的

song513

顶，有魄力！

wgl404

这个必须顶！

Oner

受教了。

Oner

受教了。                                       1

O.Z.N

好东西

我の小车

受领了。。

寂寞坏孩子9856

好人啊