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

1、巡线小车源程序(MSP430, PID)首先是各种宏定义的头文件cpp HYPERLINK l ""| view plain"myhead.h"#ifndef xxxx xxx#define xxxx xxx#define P 43#define I 0.01#define D 100#define LeftMotorTurnNeg P 3OUT|=BIT0 ;P 3OUT&=BIT1;#define LeftMotorTurn Pos P 3OUT&=BIT0 ;P 3OUT|=BIT1;#define RightMotorTurnNe

2、g P 3OUT|=BIT3 ;P 3OUT&=BIT2;10#define RightMotorTur nP os P 3OUT&=BIT3 ;P 3OUT|=BIT2;11#define LeftP wmOut TACCR112#define Right PwmOut TACCR213#define F 1000014#define T (8000000/F)15#define BASIC 10022#define Motor1617#define LEFT 100IS#define RIGHT 10019#endif#define simpi etime (unsigne

3、d int)5接下来就是点击驱动的头文件cpp2)"Motor.h"21#ifndef Motor#include "myhead.h" #include "ms p430x14x.h"void MotorLeft( floatleft)27LeftMotorTurn Po s/正转54MotorRight(right);/工作正常if (leftvO)LeftMotorTurnNeg/反转31left=-left;32LeftPwmOut=( int )(left*T/1OO);35void MotorRight( float ri

4、ght)37RightMotorTurn Po s/正转3Sif (right<0)RightMotorTurnNeg/41right=-right;Right PwmOut=(int )(right*T/100);void DriveMotor(floatP IDout)47float left=BASIC+PIDout;floatnght=BASIC-PIDout;if(left>100)left=100;foif(left<-100)left=-100;51if(right>100)right=100;52if(right<-100)right=-100;5

5、3MotorLeft(left);55 56 #endif再下来是主文件cpp57"main.c"5S#include "Motor.h"float lasterror,sumerror;61float Sensor=-4,-3,-2,-1,0,1,2,3,4;int flag;void delay( int time)void timer_inti()67TACTL=TASSEL_2+MC_1+TAR+ID_0;/SMCLK, 增计数模式，清零，不分频 ；允许定时器溢出中TACCR0=8000000/F;/ 其中 F 为 PWM勺频率TACCTL1=0

6、UTM0D_7;TACCTL2=OUTMOD_7;7172TBCTL=TASSEL_2+MC_1+TBR+ID_0+CNTL_0;/SMCLK, 增计数模式，清 零，不分频；允许定时器溢出中断，16WEI73TBCCTL0|=CCIE;15控制采样时间TBCCR0=8000*sim pietime;/75_EINT();77void IO_inti()P1DIR=BIT3+BIT2;/ 用于定时器输出soP 1SEL=BIT3+BIT2;SIP3DIR=0X7f;/用作传感器输入跟电机驱动S3P5DIR=0X00;/用于传感器输入P6DIR=0XFF;/用于流水灯输出，纯粹调试用P 2DIR|

7、=BIT4;/外部中断，用于壁障P 2IE=BIT4;P 2IES=BIT4;91void PID_inti()sumerror=0;lasterror=0;flag=0;97void Crystal。/晶振启振程序BCSCTL1 &=XT2OFF;101doint i;imIFG1 &=OFIFG;105for (i=255;i>=0;i-);1C6 while (IFG1 &OFIFG);107BCSCTL2=SELM_2+DIVM_0+SELS+DIVS_0;/SMCLK=MCLK=XT2;DIV=0;ICSl(p floatPID_Cal( float e

8、rror)126138110 111float derror=error-lasterror;/微分df112sumerror+=error;/积分113lasterror=error;114returnP *error+I*sumerro 叶D*derror;115116 float Sensor_Read()/左边传感器压线往左偏117 IISstatic float ans=0;119float sum=0;12)int i,sensor=0,num=0;121122123for (i=0;i<9;i+)sensor= P5IN;/3.7 5.7 5.6 sensor

9、=( P3IN&BIT7)vv1)|sensor;if (!(sensor&(1<<i)127sum+=Sensori;num+;131if (num!=0&&numv=4)ans=sum/num;152if (ans<0)133135P60UT=0x80;P6OUT|=( int )(-(ans-0.5);137else139P60UT=( int )(ans+0.5);141165while (1)167return ans;/ 运行找正常143144 void AvoidDetection( float left, float right

10、)/壁障函数145 int sensor=0;147MotorLeft(left);MotorRight(right);delay(200);ifodo151MotorLeft(right);152MotorRight(left);153154155 while (sensor=0x1ff);/当在空白的时候，一只往里面拐。sensor= P3IN&BIT7;/3.7 5.7 5.6 sensor=(sensor<<1)| P5IN;157 void main( void )15&/ Stop watchdog timer to p revent time out resetWDTCTL = WDT PW + WDTHOLD;161Crystal();IO_inti();timer_inti();PID_inti();165P6OUT=0XF0;if (flag)1113DriveMotoM PI D_Cal(Sensor_Read();171173 174 #pragma ve