巡线车程序(完整版,也是最先的版本)

版权声明:本文为博主原创文章,遵循 CC 4.0 by-sa 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/zc850463390zc/article/details/7548096
#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 



#include "Macro.h"
#include "sensor.h"
void Motorstop()
{
  LEFTOUT=0;
  RIGHTOUT=0;
}
void MotorLeft(int speed,int direction)
{
  if(speed>max_speed)speed=max_speed;
  if(direction==backward)//反转
  {
    EnableLeftNeg;
    UnenableLeftPos;
  }
  else if(direction==foreward)//正转
  {
    EnableLeftPos;
    UnenableLeftNeg;
  }
  LEFTOUT=Period/100*speed;
}
void MotorRight(int speed,int direction)
{
  if(speed>max_speed)speed=max_speed;
  
  if(direction==backward)//反转
  {
    EnableRightNeg;
    UnenableRightPos;
  }
  else if(direction==foreward)//正转
  {
    EnableRightPos;
    UnenableRightNeg;
  }
  RIGHTOUT=Period/100*speed;
}
void MotorDrive(int PIDout)
{
  int speedleft,speedright;
  speedleft=Basic_Left+PIDout;
  speedright=Basic_Right-PIDout;
  
  if(speedleft<0)
    MotorLeft(speedleft,backward);//反转
  else MotorLeft(speedleft,foreward);//正转
  
  if(speedright<0)
    MotorRight(speedright,backward);//反转
  else MotorRight(speedright,foreward);//正转
}
void  Rangle(float angle)
{
 // TBCTL|=TBCLR;
  TBCCR1=LMAX+(unsigned int)(angle*one); 
}




//下面是小车的程序。。用定时器A来输出两路PWM波。
//选用输出模式7(首先输出高电平。在TAR=CCRX时,电平自动变低。这样可以输出任意占空比的PWM波)
//    传感器接在P5口,电机的驱动接在P3口。
#include "Macro.h"
#include "motor.h"
typedef struct p
{
  float  error_1;//上次的误差
  float sumerror;
}PID;
PID Pid;
PID *pid=&Pid;
uchar detection,sample=0,corner,k,flag,tt=0;
int weight1[]={-8,-5,-3,-2,0,2,3,5,8};//传感器的权值
float sensorin;
int num,tag=1;
void IO_inti()//io口初始化
{
  P1SEL|=BIT2+BIT3;//p1为定时器输出端。
  P1DIR|=BIT2+BIT3;//设置为输出模式。P5为传感器输入。
  P3DIR|=BIT0+BIT1+BIT2+BIT3+BIT4;//0,1,2,3,作为电机控制的输出端,其他的作为传感器的输入端
  P5DIR=0X00;//P5为传感器输入端。设置P5为输入端。普通IO口。
  P5SEL=0x00;
  
  
  P2DIR=0X00;//输入
  
  P2IE|=BIT3;
  P2IES|=BIT3;
  
  P6DIR=0XFF;//纯粹是为了调试用的。
  P6OUT=0X00;
  
}
void delay(int t)
{
  unsigned int i;
  while(t--)
  {
    i=65535;
    while(i--);
  }
}
void CLK_inti()//时钟初始化
{
  BCSCTL1=0X00;//打开XT2
  do{
    IFG1&=~OFIFG;
    for(int i=0x20;i>0;i--);
  }while((IFG1&OFIFG)==OFIFG);//如果起震失败。。继续起震。知道成功为止
  BCSCTL2=0X00;
  BCSCTL2|=SELM_2|SELS;//mclk,sMCLK的时钟源为XT2,0分频。
}

void PID_inti()//PID初始化 
{
  pid->error_1=0;
  pid->sumerror=0;
}

void PWM_inti()//PWM初始化
{
  TACTL|=TASSEL_2|ID_0|MC_1|TACLR;//时钟源采用SMCLK,增计数模式。清空TAR,0分频
  TACCR0=8000000/F;//0分频。所以Period=8000000/5000
  TACCTL1|=OUTMOD_7;//两个PWM输出口的输出模式。
  TACCTL2|=OUTMOD_7;//
  LEFTOUT=0;
  RIGHTOUT=0;
}

float abs(float a)
{
  return a<0?-a:a;
}

void timer_inti()//用定时器B1来确定采样周期
{
  TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR;//定时器B:SMCLK,16位,8分频,增计数
  TBCCTL0|=CCIE;//使能timerb1的中断。
  TBCCR0=5000;//八分频,1MHz,5000表示5ms。
  _EINT();//打开总中断。
}
void num_inti()
{
  sensorin=0;
  num=0;
}
void inti()//总初始化函数
{
  PID_inti();
  CLK_inti();
  PWM_inti();
  IO_inti();
  timer_inti();
  num_inti();
}
float ReadSensor2()
{
  int state=0,i,num=0;
  float sum=0;
  static float output=0;
  state=P3IN&0X80;
  state<<=1;
  state|=P5IN;
  for(i=0;i<9;i++)
    if(((1<<i)&state)==0)
    {
      sum+=weight1[i];
      num++;
    }   
   if(num>0&&num<7)//不是全白或者全黑
   {
     corner=0;
     if(num>=3)
     {
       corner=1;
       if(sum>0)
         output=10;
       else output=-10;
       return output;
     }
     output=sum/num;
   }
   else if(num>=7)//如果是全黑。
   {
     flag++;
     return output=0;
   }
   return output;
}
void Avoid(int time,int lspeed,int ldirection,int rspeed,int rdirection)
{
  int state=0;
  MotorLeft(lspeed,ldirection);//左转出去,n
  delay(time);
  MotorRight(lspeed,ldirection);
  MotorLeft(rspeed,rdirection);
  do{//右转,直到传感器压线。
  state=P3IN&0X80;
  state<<=1;
  state|=P5IN;
  }while(state==0X1FF);
  PID_inti();//PID初始化
}
int  PIDCal(float error)//PID计算。位置式。
{
  float output,derror;
  
    pid->sumerror+=error;
  
  derror=(error-pid->error_1);
  
  output=error*PC+derror*DC;
  
  pid->error_1=error;
  return (int)(output+0.5);
}
void main( void )
{
  WDTCTL=WDTHOLD+WDTPW;//close the watch dog;
  inti();
  sample=0;
  corner=0;
  flag=0;
  detection=0;
  while(1)
  {
    if(sample)
    {
      sensorin=ReadSensor2();
      if(flag==1)//下坡,用定时器定时。到坡底后减速,只有第一次扫到黑线后才要减速。
      {
       // P6OUT=0X81;
        WDTCTL=(WDTPW+WDTTMSEL+WDTCNTCL);
        IE1|=WDTIE;
        flag++;
        continue;
      }
      if(corner)//如果是直角,
      {
        int state=0;
        MotorLeft(100,1);//向前冲一段时间,然后再转。避免误判
        MotorRight(100,1);
	delay_ms(20);

        if(sensorin>0)//如果是右直角
        {
            MotorLeft(100,1);
            MotorRight(30,0);
         }
         else//如果是左直角
         {
            MotorRight(100,1);
             MotorLeft(30,0); 
         }
          do
          {
            state=0;
            state=P3IN&0X80;
            state<<=1;
            state|=P5IN;     
          }while(state==0x1ff);
          continue;
        }
      MotorDrive(PIDCal(sensorin));
      sample=0;
    }
    else if(!(P2IN&0X10)&&!detection)//壁障
    {
      detection=1;
      P6OUT=0XF0;
      MotorLeft(100,0);
      MotorRight(100,0);
      delay_ms(50);
      if(pid->sumerror<0)
      Avoid(5,60,0,100,1);//先左转,再右转
      else Avoid(5,100,1,60,0);//先右转,再左转
    }
  }
}
#pragma vector=TIMERB0_VECTOR
__interrupt void TimerB()//控制采样周期,大概10ms,中断正常。。
{
  sample=1;
}
#pragma vector=PORT2_VECTOR//用外部中断来判断并且处理避障
__interrupt void Outside()//最后的抓取。用定时器B才给舵机输出PWM波
{
  if(P2IFG&BIT3)
  {
    P2IFG&=~BIT3;
  //  P6OUT=0X07;
    TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR|TBIE;//定时器B:SMCLK,16位,4分频,增计数
    TBCCR0=39999;//0分频。所以Period=8000000/500=16000
    TBCCTL1|=OUTMOD_7;//两个PWM输出口的输出模式。
    TBCCR1=0;
    P4DIR|=BIT1;
    P4SEL|=BIT1;
    MotorLeft(0,1);//停止
    MotorRight(0,1);
    delay_ms(50);//延迟
    Rangle(0);//夹住东西
    delay_ms(300);
    MotorLeft(100,0);//后退
    MotorRight(100,0);
    delay_ms(200);//延迟
    MotorLeft(0,0);
    MotorRight(0,0);
    while(1);//死循环,结束。
  }
}
#pragma vector=WDT_VECTOR
__interrupt void wdtch()
{
  static int xx=0;
  if(++xx==300)
  {
  //  P6OUT=0x01;
    MotorLeft(100,0);
    MotorRight(100,0);
    delay_ms(160);
    WDTCTL=WDTHOLD+WDTPW;
    IE1&=~WDTIE;
    xx=0;
    tag=0;
  }
}


展开阅读全文

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