巡线小车源程序(MSP430,PID)

首先是各种宏定义的头文件


#ifndef _Macro.h_
#define _Macro.h_   
#include <msp430x14x.h>
#include <intrinsics.h>
#define uchar unsigned char 
#define uint unsigned int
#define one 11.11
#define LMAX 1999
#define RMAX 3999
#define CPU_F ((double)8000000)
#define delay_us(x) __delay_cycles((long)(CPU_F*(double)x/1000000.0))

#define delay_ms(x) __delay_cycles((long)(CPU_F*(double)x/1000.0))
#define PC 20  // 比例放大系数
#define IC 0    //积分放大系数
#define DC 85    //大系数
#define LEFTOUT    TACCR1
#define RIGHTOUT   TACCR2
#define SensorIn   P5IN
#define F          5000//5000hz
#define Period     (8000000/F)
#define EnableLeftPos       P3OUT|=BIT1
#define UnenableLeftPos     P3OUT&=~BIT1

#define EnableLeftNeg       P3OUT|=BIT0
#define UnenableLeftNeg     P3OUT&=~BIT0

#define EnableRightPos      P3OUT|=BIT2
#define UnenableRightPos    P3OUT&=~BIT2

#define EnableRightNeg      P3OUT|=BIT3
#define UnenableRightNeg    P3OUT&=~BIT3

#define Basic_Left  100//百分之八十
#define Basic_Right 100//Basic_Left
#define MAX        (100)
#define MIN        (-100)
#define foreward   1
#define backward   0
#define max_speed  100
#define min_speed  -100
#define key        0


#define left_1 1
#define left_2 2
#define left_3 3
#define left_4 4
#define left_5 5
#define left_6 6
#define left_7 7//右直角

#define right_1 -1
#define right_2 -2
#define right_3 -3
#define right_4 -4
#define right_5 -5
#define right_6 -6
#define right_7 -7//左直角
#endif 

接下来就是点击驱动的头文件


"Motor.h"
#ifndef Motor
#define Motor
#include "myhead.h"
#include "msp430x14x.h"
void MotorLeft(float left)
{
  LeftMotorTurnPos//正转
  if(left<0)
  {
    LeftMotorTurnNeg//反转
    left=-left;
  }
  LeftPwmOut=(int)(left*T/100);
}
void MotorRight(float right)
{
  RightMotorTurnPos//正转
  if(right<0)
  {
    RightMotorTurnNeg//
    right=-right;
  }
   RightPwmOut=(int)(right*T/100);
}
void DriveMotor(float PIDout)
{
    float left=BASIC+PIDout;
    float right=BASIC-PIDout;
    if(left>100)left=100;
    if(left<-100)left=-100;
    if(right>100)right=100;
    if(right<-100)right=-100;
    MotorLeft(left);
    MotorRight(right);//工作正常
}
#endif

再下来是主文件

”main.c"

#include "Motor.h"
float lasterror,sumerror;
float Sensor[]={-4,-3,-2,-1,0,1,2,3,4};
int flag;
void delay(int time)
{}
void timer_inti()
{
  TACTL=TASSEL_2+MC_1+TAR+ID_0;//SMCLK,增计数模式,清零,不分频;允许定时器溢出中断
  TACCR0=8000000/F;//其中F为PWM的频率
  TACCTL1=OUTMOD_7;
  TACCTL2=OUTMOD_7;
  
  TBCTL=TASSEL_2+MC_1+TBR+ID_0+CNTL_0;//SMCLK,增计数模式,清 零,不分频;允许定时器溢出中断,16WEI
  TBCCTL0|=CCIE;
  TBCCR0=8000*simpletime;//控制采样时间
  _EINT();
}
void IO_inti()
{
  P1DIR=BIT3+BIT2;//用于定时器输出
  P1SEL=BIT3+BIT2;
  
  P3DIR=0X7f;//用作传感器输入跟电机驱动
  P5DIR=0X00;//用于传感器输入
  
  P6DIR=0XFF;//用于流水灯输出,纯粹调试用
  
  P2DIR|=BIT4;//外部中断,用于壁障
  P2IE=BIT4;
  P2IES=BIT4;
  
}
void PID_inti()
{
  sumerror=0;
  lasterror=0;
  flag=0;
}
void Crystal()//晶振启振程序
{
  BCSCTL1&=~XT2OFF;
  do
  {
    int i;
    IFG1&=~OFIFG;
    for(i=255;i>=0;i--);
  }while(IFG1&OFIFG);
  BCSCTL2=SELM_2+DIVM_0+SELS+DIVS_0;//SMCLK=MCLK=XT2;DIV=0;
}
float PID_Cal(float error)
{
  float derror=error-lasterror;//微分df
  sumerror+=error;//积分
  lasterror=error;
  return P*error+I*sumerror+D*derror;
}
float Sensor_Read()//左边传感器压线往左偏
{
  static float ans=0;
  float sum=0;
  int i,sensor=0,num=0;
  sensor=P5IN;//3.7 5.7 5.6 5.5.....5.0
  sensor=((P3IN&BIT7)<<1)|sensor;
  for(i=0;i<9;i++)
  {
    if(!(sensor&(1<<i)))
    {
      sum+=Sensor[i];
      num++;
    }
  }
  if(num!=0&&num<=4)ans=sum/num;
  if(ans<0)
  {
    P6OUT=0x80;
    P6OUT|=(int)(-(ans-0.5));
  }
  else 
  {
    P6OUT=(int)(ans+0.5);
  }
  
  return ans;//运行找正常
}
void AvoidDetection(float left,float right)//壁障函数
{
  int sensor=0;
  MotorLeft(left);
  MotorRight(right);
  delay(200);
  do{
    MotorLeft(right);
    MotorRight(left);
    sensor=P3IN&BIT7;//3.7 5.7 5.6 5.5.....5.0
    sensor=(sensor<<1)|P5IN;
    }while(sensor==0x1ff);//当在空白的时候,一只往里面拐。
}
void main( void )
{
  // Stop watchdog timer to prevent time out reset
  WDTCTL = WDTPW + WDTHOLD;
  Crystal();
  IO_inti();
  timer_inti();
  PID_inti();
  P6OUT=0XF0;
  while(1)
  {
    if(flag)
    {
      DriveMotor(PID_Cal(Sensor_Read()));
    }
  }
}
#pragma vector=TIMERB0_VECTOR
__interrupt void timer_bo()
{
  flag=!flag;
}
#pragma vector=PORT1_VECTOR
__interrupt void port_1()
{
  AvoidDetection(LEFT,RIGHT);
}

转载出处:http://blog.csdn.net/zc850463390zc/article/details/7540766
展开阅读全文

没有更多推荐了,返回首页