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

巡线小车源程序（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&=BIT2; 10 #define RightMotorTurnPos P3OUT&=BIT3;P3OUT|=BIT2; 11 #define LeftPwmOut TACCR1 12 #define RightPwmOut TACCR2 13 #define F 10000 14 #define T (8000000/F) 15 #define BASIC 100 16 #define simpletime (unsigned int)5 17 #define LEFT 100 18 #define RIGHT 100 19 #endif 接下来就是点击驱动的头文件 cppview plaincopyprint?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/正转 28 if(left0) 29 30 LeftMotorTurnNeg/反转 31 left=-left; 32 33 LeftPwmOut=(int)(left*T/100); 34 35 void MotorRight(float right) 36 37 RightMotorTurnPos/正转 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*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&num=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)/壁障函数 145 146 int sensor=0; 147 MotorLeft(left); 148 MotorRight(right); 149 delay(200); 150 do 151 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