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

巡线小车源程序（MSP430，PID）首先是各种宏定义的头文件[cpp]viewplaincopymyhead.h#ifndefxxxx_xxx#definexxxx_xxx#defineP43#defineI0.01#defineD100#defineLeftMotorTurnNeg{P3OUT|=BIT0;P3OUT&=~BIT1;}#defineLeftMotorTurnPos{P3OUT&=~BIT0;P3OUT|=BIT1;}#defineRightMotorTurnNeg{P3OUT|=BIT3;P3OUT&=~BIT2;}#defineRightMotorTurnPos{P3OUT&=~BIT3;P3OUT|=BIT2;}#defineLeftPwmOutTACCR1#defineRightPwmOutTACCR2#defineF10000#defineT(8000000/F)#defineBASIC100#definesimpletime(unsignedint)5#defineLEFT100#defineRIGHT100#endif接下来就是点击驱动的头文件[cpp]viewplaincopyMotor.h#ifndefMotor#defineMotor#includemyhead.h#includemsp430x14x.hvoidMotorLeft(floatleft){LeftMotorTurnPos//正转if(left0){LeftMotorTurnNeg//反转left=-left;}LeftPwmOut=(int)(left*T/100);}voidMotorRight(floatright){RightMotorTurnPos//正转if(right0){RightMotorTurnNeg//right=-right;}RightPwmOut=(int)(right*T/100);}voidDriveMotor(floatPIDout){floatleft=BASIC+PIDout;floatright=BASIC-PIDout;if(left100)left=100;if(left-100)left=-100;if(right100)right=100;if(right-100)right=-100;MotorLeft(left);MotorRight(right);//工作正常}#endif再下来是主文件[cpp]viewplaincopy”main.c#includeMotor.hfloatlasterror,sumerror;floatSensor[]={-4,-3,-2,-1,0,1,2,3,4};intflag;voiddelay(inttime){}voidtimer_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();}voidIO_inti(){P1DIR=BIT3+BIT2;//用于定时器输出P1SEL=BIT3+BIT2;P3DIR=0X7f;//用作传感器输入跟电机驱动P5DIR=0X00;//用于传感器输入P6DIR=0XFF;//用于流水灯输出,纯粹调试用P2DIR|=BIT4;//外部中断，用于壁障P2IE=BIT4;P2IES=BIT4;}voidPID_inti(){sumerror=0;lasterror=0;flag=0;}voidCrystal()//晶振启振程序{BCSCTL1&=~XT2OFF;do{inti;IFG1&=~OFIFG;for(i=255;i=0;i--);}while(IFG1&OFIFG);BCSCTL2=SELM_2+DIVM_0+SELS+DIVS_0;//SMCLK=MCLK=XT2;DIV=0;}floatPID_Cal(floaterror){floatderror=error-lasterror;//微分dfsumerror+=error;//积分lasterror=error;returnP*error+I*sumerror+D*derror;}floatSensor_Read()//左边传感器压线往左偏{staticfloatans=0;floatsum=0;inti,sensor=0,num=0;sensor=P5IN;//3.75.75.65.5.....5.0sensor=((P3IN&BIT7)1)|sensor;for(i=0;i9;i++){if(!(sensor&(1i))){sum+=Sensor[i];num++;}}if(num!=0&&num=4)ans=sum/num;if(ans0){P6OUT=0x80;P6OUT|=(int)(-(ans-0.5));}else{P6OUT=(int)(ans+0.5);}returnans;//运行找正常}voidAvoidDetection(floatleft,floatright)//壁障函数{intsensor=0;MotorLeft(left);MotorRight(right);delay(200);do{MotorLeft(right);MotorRight(left);sensor=P3IN&BIT7;//3.75.75.65.5.....5.0sensor=(sensor1)|P5IN;}while(sensor==0x1ff);//当在空白的时候，一只往里面拐。}voidmain(void){//StopwatchdogtimertopreventtimeoutresetWDTCTL=WDTPW+WDTHOLD;Crystal();IO_inti();timer_inti();PID_inti();P6OUT=0XF0;while(1){if(flag){DriveMotor(PID_Cal(Sensor_Read()));}}}#pragmavector=TIMERB0_VECTOR__interruptvoidtimer_bo(){flag=!flag;}#pragmavector=PORT1_VECTOR__interruptvoidport_1(){AvoidDetection(LEFT,RIGHT);}