巡线车程序（完整版)
1 #ifndef _Macro.h_
2 #define _Macro.h_
3 #include
4 #include
5 #define uchar unsigned char
6 #define uint unsigned int
7 #define one 11.11
8 #define LMAX 1999
9 #define RMAX 3999
10 #define CPU_F ((double)8000000)
11 #define delay_us(x) __delay_cycles((long)(CPU_F*(double)x/1000000.0))
12
13 #define delay_ms(x) __delay_cycles((long)(CPU_F*(double)x/1000.0))
14 #define PC 20 // 比例放大系数
15 #define IC 0 //积分放大系数
16 #define DC 85 //大系数
17 #define LEFTOUT TACCR1
18 #define RIGHTOUT TACCR2
19 #define SensorIn P5IN
20 #define F 5000//5000hz
21 #define Period (8000000/F)
22 #define EnableLeftPos P3OUT|=BIT1
23 #define UnenableLeftPos P3OUT&=~BIT1
24
25 #define EnableLeftNeg P3OUT|=BIT0
26 #define UnenableLeftNeg P3OUT&=~BIT0
27
28 #define EnableRightPos P3OUT|=BIT2
29 #define UnenableRightPos P3OUT&=~BIT2
30
31 #define EnableRightNeg P3OUT|=BIT3
32 #define UnenableRightNeg P3OUT&=~BIT3
33
34 #define Basic_Left 100//百分之八十
35 #define Basic_Right 100//Basic_Left
36 #define MAX (100)
37 #define MIN (-100)
38 #define foreward 1
39 #define backward 0
40 #define max_speed 100
41 #define min_speed -100
42 #define key 0
43
44
45 #define left_1 1
46 #define left_2 2
47 #define left_3 3
48 #define left_4 4
49 #define left_5 5
50 #define left_6 6
51 #define left_7 7//右直角
52
53 #define right_1 -1
54 #define right_2 -2
55 #define right_3 -3
56 #define right_4 -4
57 #define right_5 -5
58 #define right_6 -6
59 #define right_7 -7//左直角
60 #endif
[cpp]view plaincopy
61 #include "Macro.h"
62 #include "sensor.h"
63 void Motorstop()
64 {
65 LEFTOUT=0;
66 RIGHTOUT=0;
67 }
68 void MotorLeft(int speed,int direction)
69 {
70 if(speed>max_speed)speed=max_speed;
71 if(direction==backward)//反转
72 {
73 EnableLeftNeg;
74 UnenableLeftPos;
75 }
76 else if(direction==foreward)//正转
77 {
78 EnableLeftPos;
79 UnenableLeftNeg;
80 }
81 LEFTOUT=Period/100*speed;
82 }
83 void MotorRight(int speed,int direction)
84 {
85 if(speed>max_speed)speed=max_speed;
86
87 if(direction==backward)//反转
88 {
89 EnableRightNeg;
90 UnenableRightPos;
91 }
92 else if(direction==foreward)//正转
93 {
94 EnableRightPos;
95 UnenableRightNeg;
96 }
97 RIGHTOUT=Period/100*speed;
98 }
99 void MotorDrive(int PIDout)
100 {
101 int speedleft,speedright;
102 speedleft=Basic_Left+PIDout;
103 speedright=Basic_Right-PIDout;
104
105 if(speedleft<0)
106 MotorLeft(speedleft,backward);//反转
107 else MotorLeft(speedleft,foreward);//正转
108
109 if(speedright<0)
110 MotorRight(speedright,backward);//反转
111 else MotorRight(speedright,foreward);//正转
112 }
113 void Rangle(float angle)
114 {
115 // TBCTL|=TBCLR;
116 TBCCR1=LMAX+(unsigned int)(angle*one);
117 }
[cpp]view plaincopy
118 //下面是小车的程序。。用定时器A来输出两路PWM波。
119 //选用输出模式7（首先输出高电平。在TAR=CCRX时，电平自动变低。这样可以输出任意占空比的PWM波）
120 // 传感器接在P5口，电机的驱动接在P3口。
121 #include "Macro.h"
122 #include "motor.h"
123 typedef struct p
124 {
125 float error_1;//上次的误差
126 float sumerror;
127 }PID;
128 PID Pid;
129 PID *pid=&Pid;
130 uchar detection,sample=0,corner,k,flag,tt=0;
131 int weight1[]={-8,-5,-3,-2,0,2,3,5,8};//传感器的权值
132 float sensorin;
133 int num,tag=1;
134 void IO_inti()//io口初始化
135 {
136 P1SEL|=BIT2+BIT3;//p1为定时器输出端。
137 P1DIR|=BIT2+BIT3;//设置为输出模式。P5为传感器输入。
138 P3DIR|=BIT0+BIT1+BIT2+BIT3+BIT4;//0,1,2,3,作为电机控制的输出端,其他的作为传感器的输入端
139 P5DIR=0X00;//P5为传感器输入端。设置P5为输入端。普通IO口。
140 P5SEL=0x00;
141
142
143 P2DIR=0X00;//输入
144
145 P2IE|=BIT3;
146 P2IES|=BIT3;
147
148 P6DIR=0XFF;//纯粹是为了调试用的。
149 P6OUT=0X00;
150
151 }
152 void delay(int t)
153 {
154 unsigned int i;
155 while(t--)
156 {
157 i=65535;
158 while(i--);
159 }
160 }
161 void CLK_inti()//时钟初始化
162 {
163 BCSCTL1=0X00;//打开XT2
164 do{
165 IFG1&=~OFIFG;
166 for(int i=0x20;i>0;i--);
167 }while((IFG1&OFIFG)==OFIFG);//如果起震失败。。继续起震。知道成功为止
168 BCSCTL2=0X00;
169 BCSCTL2|=SELM_2|SELS;//mclk,sMCLK的时钟源为XT2，0分频。
170 }
171
172 void PID_inti()//PID初始化
173 {
174 pid->error_1=0;
175 pid->sumerror=0;
176 }
177
178 void PWM_inti()//PWM初始化
179 {
180 TACTL|=TASSEL_2|ID_0|MC_1|TACLR;//时钟源采用SMCLK,增计数模式。清空TAR，0分频
181 TACCR0=8000000/F;//0分频。所以Period=8000000/5000
182 TACCTL1|=OUTMOD_7;//两个PWM输出口的输出模式。
183 TACCTL2|=OUTMOD_7;//
184 LEFTOUT=0;
185 RIGHTOUT=0;
186 }
187
188 float abs(float a)
189 {
190 return a<0?-a:a;
191 }
192
193 void timer_inti()//用定时器B1来确定采样周期
194 {
195 TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR;//定时器B：SMCLK,16位，8分频,增计数
196 TBCCTL0|=CCIE;//使能timerb1的中断。
197 TBCCR0=5000;//八分频，1MHz，5000表示5ms。
198 _EINT();//打开总中断。
199 }
200 void num_inti()
201 {
202 sensorin=0;
203 num=0;
204 }
205 void inti()//总初始化函数
206 {
207 PID_inti();
208 CLK_inti();
209 PWM_inti();
210 IO_inti();
211 timer_inti();
212 num_inti();
213 }
214 float ReadSensor2()
215 {
216 int state=0,i,num=0;
217 float sum=0;
218 static float output=0;
219 state=P3IN&0X80;
220 state<<=1;
221 state|=P5IN;
222 for(i=0;i<9;i++)
223 if(((1<
225 sum+=weight1[i];
226 num++;
227 }
228 if(num>0&&num<7)//不是全白或者全黑
229 {
230 corner=0;
231 if(num>=3)
232 {
233 corner=1;
234 if(sum>0)
235 output=10;
236 else output=-10;
237 return output;
238 }
239 output=sum/num;
240 }
241 else if(num>=7)//如果是全黑。
242 {
243 flag++;
244 return output=0;
245 }
246 return output;
247 }
248 void Avoid(int time,int lspeed,int ldirection,int rspeed,int rdirection)
249 {
250 int state=0;
251 MotorLeft(lspeed,ldirection);//左转出去，n
252 delay(time);
253 MotorRight(lspeed,ldirection);
254 MotorLeft(rspeed,rdirection);
255 do{//右转，直到传感器压线。
256 state=P3IN&0X80;
257 state<<=1;
258 state|=P5IN;
259 }while(state==0X1FF);
260 PID_inti();//PID初始化
261 }
262 int PIDCal(float error)//PID计算。位置式。
263 {
264 float output,derror;
265
266 pid->sumerror+=error;
267
268 derror=(error-pid->error_1);
269
270 output=error*PC+derror*DC;
271
272 pid->error_1=error;
273 return (int)(output+0.5);
274 }
275 void main( void )
276 {
277 WDTCTL=WDTHOLD+WDTPW;//close the watch dog;
278 inti();
279 sample=0;
280 corner=0;
281 flag=0;
282 detection=0;
283 while(1)
284 {
285 if(sample)
286 {
287 sensorin=ReadSensor2();
288 if(flag==1)//下坡，用定时器定时。到坡底后减速，只有第一次扫到黑线后才要减速。
289 {
290 // P6OUT=0X81;
291 WDTCTL=(WDTPW+WDTTMSEL+WDTCNTCL);
292 IE1|=WDTIE;
293 flag++;
294 continue;
295 }
296 if(corner)//如果是直角，
297 {
298 int state=0;
299 MotorLeft(100,1);//向前冲一段时间，然后再转。避免误判
300 MotorRight(100,1);
301 delay_ms(20);
302
303 if(sensorin>0)//如果是右直角
304 {
305 MotorLeft(100,1);
306 MotorRight(30,0);
307 }
308 else//如果是左直角
309 {
310 MotorRight(100,1);
311 MotorLeft(30,0);
312 }
313 do
314 {
315 state=0;
316 state=P3IN&0X80;
317 state<<=1;
318 state|=P5IN;
319 }while(state==0x1ff);
320 continue;
321 }
322 MotorDrive(PIDCal(sensorin));
323 sample=0;
324 }
325 else if(!(P2IN&0X10)&&!detection)//壁障
326 {
327 detection=1;
328 P6OUT=0XF0;
329 MotorLeft(100,0);
330 MotorRight(100,0);
331 delay_ms(50);
332 if(pid->sumerror<0)
333 Avoid(5,60,0,100,1);//先左转，再右转
334 else Avoid(5,100,1,60,0);//先右转，再左转
335 }
336 }
337 }
338 #pragma vector=TIMERB0_VECTOR
339 __interrupt void TimerB()//控制采样周期，大概10ms,中断正常。。
340 {
341 sample=1;
342 }
343 #pragma vector=PORT2_VECTOR//用外部中断来判断并且处理避障
344 __interrupt void Outside()//最后的抓取。用定时器B才给舵机输出PWM波
345 {
346 if(P2IFG&BIT3)
347 {
348 P2IFG&=~BIT3;
349 // P6OUT=0X07;
350 TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR|TBIE;//定时器B：SMCLK,16位，4分频,增计数
351 TBCCR0=39999;//0分频。所以Period=8000000/500=16000
352 TBCCTL1|=OUTMOD_7;//两个PWM输出口的输出模式。
353 TBCCR1=0;
354 P4DIR|=BIT1;
355 P4SEL|=BIT1;
356 MotorLeft(0,1);//停止
357 MotorRight(0,1);
358 delay_ms(50);//延迟
359 Rangle(0);//夹住东西
360 delay_ms(300);
361 MotorLeft(100,0);//后退
362 MotorRight(100,0);
363 delay_ms(200);//延迟
364 MotorLeft(0,0);
365 MotorRight(0,0);
366 while(1);//死循环，结束。
367 }
368 }
369 #pragma vector=WDT_VECTOR
370 __interrupt void wdtch()
371 {
372 static int xx=0;
373 if(++xx==300)
374 {
375 // P6OUT=0x01;
376 MotorLeft(100,0);
377 MotorRight(100,0);
378 delay_ms(160);
379 WDTCTL=WDTHOLD+WDTPW;
380 IE1&=~WDTIE;
381 xx=0;
382 tag=0;
383 }
384 }