巡線小車源程序(MSP430-PID

1、巡線小車源程序（MSP430，PID）首先是各種宏定義的頭文件cppview plaincopyprint?1 myhead.h 2 #ifndef xxxx_xxx 3 #define xxxx_xxx 4 #define P 43 5 #define I 0.01 6 #define D 100 7 #define LeftMotorTurnNeg P3OUT|=BIT0;P3OUT&=BIT1; 8 #define LeftMotorTurnPos P3OUT&=BIT0;P3OUT|=BIT1; 9 #define RightMotorTurnNeg P3OUT|=BIT3;P3OUT

2、&=BIT2; 10 #define RightMotorTurnPos P3OUT&=BIT3;P3OUT|=BIT2; 11 #define LeftPwmOut TACCR1 12 #define RightPwmOut TACCR2 13 #define F 10000 14 #define T (/F) 15 #define BASIC 100 16 #define simpletime (unsigned int)5 17 #define LEFT 100 18 #define RIGHT 100 19 #endif 接下來就是點擊驅(qū)動的頭文件 cppview plaincopyp

3、rint?20 Motor.h 21 #ifndef Motor 22 #define Motor 23 #include myhead.h 24 #include msp430x14x.h 25 void MotorLeft(float left) 26 27 LeftMotorTurnPos/正轉(zhuǎn) 28 if(left0) 29 30 LeftMotorTurnNeg/反轉(zhuǎn) 31 left=-left; 32 33 LeftPwmOut=(int)(left*T/100); 34 35 void MotorRight(float right) 36 37 RightMotorTurnPos

4、/正轉(zhuǎn) 38 if(right100)left=100; 50 if(left100)right=100; 52 if(right=0;i-); 106 while(IFG1&OFIFG); 107 BCSCTL2=SELM_2+DIVM_0+SELS+DIVS_0;/SMCLK=MCLK=XT2;DIV=0; 108 109 float PID_Cal(float error) 110 111 float derror=error-lasterror;/微分df 112 sumerror+=error;/積分 113 lasterror=error; 114 return P*error+I

5、*sumerror+D*derror; 115 116 float Sensor_Read()/左邊傳感器壓線往左偏 117 118 static float ans=0; 119 float sum=0; 120 int i,sensor=0,num=0; 121 sensor=P5IN;/3.7 5.7 5.6 122 sensor=(P3IN&BIT7)1)|sensor; 123 for(i=0;i9;i+) 124 125 if(!(sensor&(1i) 126 127 sum+=Sensori; 128 num+; 129 130 131 if(num!=0&nu

6、m=4)ans=sum/num; 132 if(ans0) 133 134 P6OUT=0x80; 135 P6OUT|=(int)(-(ans-0.5); 136 137 else 138 139 P6OUT=(int)(ans+0.5); 140 141 142 return ans;/運行找正常 143 144 void AvoidDetection(float left,float right)/壁障函數(shù) 145 146 int sensor=0; 147 MotorLeft(left); 148 MotorRight(right); 149 delay(200); 150 do 15

7、1 MotorLeft(right); 152 MotorRight(left); 153 sensor=P3IN&BIT7;/3.7 5.7 5.6 154 sensor=(sensor1)|P5IN; 155 while(sensor=0x1ff);/當在空白的時候，一只往里面拐。 156 157 void main( void ) 158 159 / Stop watchdog timer to prevent time out reset 160 WDTCTL = WDTPW + WDTHOLD; 161 Crystal(); 162 IO_inti(); 163 timer_inti(); 164 PID_inti(); 165 P6OUT=0XF0; 166 while(1) 167 168 if(flag) 169 170 DriveMotor(PID_Cal(Sensor_Read(); 171 172 173 174 #pragma vector=TIMERB0_VECTOR 175 _interrupt void timer_bo() 176 1