msp430实现小车PID巡线控制

guethl

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

"myhead.h"
#ifndef xxxx_xxx
#define xxxx_xxx
#define P  43
#define I  0.01
#define D  100
#define LeftMotorTurnNeg   {P3OUT|=BIT0;P3OUT&=~BIT1;}
#define LeftMotorTurnPos   {P3OUT&=~BIT0;P3OUT|=BIT1;}
#define RightMotorTurnNeg  {P3OUT|=BIT3;P3OUT&=~BIT2;}
#define RightMotorTurnPos  {P3OUT&=~BIT3;P3OUT|=BIT2;}
#define LeftPwmOut  TACCR1
#define RightPwmOut TACCR2
#define F    10000
#define T    (8000000/F)
#define BASIC  100
#define simpletime (unsigned int)5
#define LEFT    100
#define RIGHT   100
#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);
}