智能车制作

标题: 电磁源代码 [打印本页]

作者: 943294069 时间: 2011-10-3 16:33
标题: 电磁源代码
留下来，发挥点余热，仅作参考

#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 时间: 2011-10-3 16:34
//------------------道路分析-----------------//
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 时间: 2011-10-4 23:18
谢谢，参考共享。想请问一下，你的电磁是怎样排布的

作者: song513 时间: 2011-10-7 19:45
顶，有魄力！

作者: wgl404 时间: 2011-10-8 15:02
这个必须顶！

作者: Oner 时间: 2011-10-8 22:04
受教了。

作者: Oner 时间: 2011-10-8 22:04
受教了。 1

作者: O.Z.N 时间: 2011-10-13 22:31
好东西

作者: 我の小车 时间: 2011-10-14 11:06
受领了。。

作者: 寂寞坏孩子9856 时间: 2011-10-20 01:58
好人啊

作者: xiaoG 时间: 2011-11-9 21:47
诅咒看此贴不回的人做出车来跑不了

作者: walkwithrain 时间: 2011-11-15 00:04
这个必须有！

作者: chen19910528 时间: 2011-11-15 11:10
支持楼主一下！！

作者: 刘世林 时间: 2011-11-15 13:26
顶

作者: sutaihua 时间: 2011-11-21 21:46
我喜欢。。。

作者: 2009KONE 时间: 2011-11-21 22:53
楼主好人啊

作者: 天才大空翼 时间: 2011-11-21 23:02
不错，可惜，电磁车要直立了。

作者: 断翅at雄鹰 时间: 2011-11-23 14:29
看了

作者: jyh728 时间: 2011-11-24 00:38
怎么感觉不全啊。。。顺便说一下传感器布局啊

作者: 664626482 时间: 2011-11-24 23:09
给力，你们用的电机驱动是什么

作者: 943294069 时间: 2012-1-17 15:24
注意：这程序是第六届电磁组的，速度2.9m/s

作者: 943294069 时间: 2012-1-17 15:27
fasong变量为蓝牙使用，发送数据回上位机

作者: 943294069 时间: 2012-1-17 15:29
传感器8个，一排

作者: 王谦623 时间: 2012-2-14 09:49
非常感谢！

作者: 春风 时间: 2012-2-18 14:16
这样也行呀。

作者: 有-木-友 时间: 2012-2-18 14:25
好孩子！

作者: zhu_xuekui 时间: 2012-2-21 01:38
牛牛牛

作者: linping9656 时间: 2012-2-27 21:34
哎，我都不会

作者: pcj2011 时间: 2012-2-27 22:27
mark

作者: 圣徒 时间: 2012-3-10 09:46
好人好人啊太感谢了

作者: 影之歌 时间: 2012-3-11 01:11
好强大~！顶~！

作者: sincelh 时间: 2012-3-25 00:23
呃看一下

作者: f43 时间: 2012-3-25 08:58
谢谢楼主啊

作者: 涛涛 时间: 2012-3-25 09:35

谢谢楼主！！

作者: 涛涛 时间: 2012-3-25 09:35

哈哈哈~~~

作者: zhangwenqiang 时间: 2012-7-1 13:49
MARK

作者: 精忠报国 时间: 2012-7-18 11:23
受领了。。

作者: 张路军 时间: 2012-8-29 21:25
好东西！楼主好人！

作者: HAWX 时间: 2014-11-30 20:41
太感谢了

作者: 13862773798 时间: 2016-3-3 20:04
谢！！！

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