基于07组委会的（简化）四驱源程序代码（未完）（武汉工程大学kcmxi）

制作爱好者

1. /****************************************************************************/
   
2. /*                        "kit07_4wd.c"       （武汉工程大学-copyright ）                 */
   
3. /*                                                                                      2009.07                      */
   
4. /****************************************************************************/
   
5. 
   
6. /*======================================*/
   
7. /*                         Include                                                 */
   
8. /*======================================*/
   
9. #include    <machine.h>
   
10. #include    "h8_3048.h"
    
11. 
    
12. 
    
13. #define         PWM_CYCLE       49151   /* PWM 16ms       */
    
14.                                                                      /* PWM_CYCLE =              */
    
15.                                                                      /*      16[ms] / 325.5[ns]  */
    
16.                                                                      /*               = 49152    */
    
17. #define         F_PWM_CYCLE       3071                                         
    
18. #define         SERVO_CENTER    5000   
    
19. #define         HANDLE_STEP         26     
    
20. 
    
21. 
    
22. 
    
23. #define         MASK2_2         0x66   
    
24. #define         MASK2_0         0x60   
    
25. #define         MASK0_2         0x06   
    
26. #define         MASK3_3         0xe7   
    
27. #define         MASK0_3         0x07   
    
28. #define         MASK3_0         0xe0   
    
29. #define         MASK4_0         0xf0   
    
30. #define         MASK0_4         0x0f   
    
31. #define         MASK4_4         0xff   
    
32. 
    
33. #define         speed_r(x,y)    speed(x,y)  /* speed  = speed_r     */
    
34. #define         speed2_r(x,y)   speed2(x,y) /* speed2 = speed2_r    */
    
35. 
    
36. void init( void );
    
37. void timer( unsigned long timer_set );
    
38. int check_crossline( void );
    
39. int check_rightline( void );
    
40. int check_leftline( void );
    
41. unsigned char sensor_inp( unsigned char mask );
    
42. unsigned char dipsw_get( void );
    
43. unsigned char pushsw_get( void );
    
44. unsigned char startbar_get( void );
    
45. void led_out( unsigned char led );
    
46. void speed( int accele_l, int accele_r );
    
47. void speed2( int accele_l, int accele_r );
    
48. void speed_f( int accele_l, int accele_r );
    
49. void speed2_f( int accele_l, int accele_r );
    
50. void handle( int angle );
    
51. 
    
52. unsigned long   cnt0;                  
    
53. unsigned long   cnt1;                 
    
54. int             pattern;               
    
55. 
    
56. void main( void )
    
57. {
    
58.     int     i;
    
59.     int     m1, m2, m3, m4;
    
60.     unsigned char c;
    
61. 
    
62.     init();                           
    
63.     set_ccr( 0x00 );                 
    
64. 
    
65.     handle( 0 );
    
66.     speed( 0, 0 );
    
67. 
    
68.     while( 1 ) {
    
69.         if( cnt1 < 1000 ) {
    
70.             led_out( 0x1 );
    
71.             i = 50;
    
72.         } else if( cnt1 < 2000 ) {
    
73.             led_out( 0x0 );
    
74.             i = 0;
    
75.         } else if( cnt1 < 3000 ) {
    
76.             led_out( 0x2 );
    
77.             i = -50;
    
78.         } else if( cnt1 < 4000 ) {
    
79.             led_out( 0x0 );
    
80.             i = 0;
    
81.         } else {
    
82.             cnt1 = 0;
    
83.         }
    
84.         c = dipsw_get();
    
85.         if( c & 0x8) m1 = i; else m1 = 0;
    
86.         if( c & 0x4) m2 = i; else m2 = 0;
    
87.         if( c & 0x2) m3 = i; else m3 = 0;
    
88.         if( c & 0x1) m4 = i; else m4 = 0;
    
89.         speed2_f( m1, m2 );         
    
90.         speed2_r( m3, m4 );         
    
91.     }
    
92. 
    
93.     while( 1 ) {
    
94.     switch( pattern ) {
    
95. 
    
96.        case 0:
    
97.         if( pushsw_get() ) {
    
98.             pattern = 1;
    
99.             cnt1 = 0;
    
100.             break;
     
101.         }
     
102.         if( cnt1 < 100 ) {           
     
103.             led_out( 0x1 );
     
104.         } else if( cnt1 < 200 ) {
     
105.             led_out( 0x2 );
     
106.         } else {
     
107.             cnt1 = 0;
     
108.         }
     
109.         break;
     
110.     case 1:
     
111.      
     
112.         if( !startbar_get() ) {
     
113.             
     
114.             led_out( 0x0 );
     
115.             pattern = 11;
     
116.             cnt1 = 0;
     
117.             break;
     
118.         }
     
119.         if( cnt1 < 50 ) {      
     
120.             led_out( 0x1 );
     
121.         } else if( cnt1 < 100 ) {
     
122.             led_out( 0x2 );
     
123.         } else {
     
124.             cnt1 = 0;
     
125.         }
     
126.         break;
     
127.     case 11:
     
128.    
     
129.         if( check_crossline() ) {      
     
130.             pattern = 21;
     
131.             break;
     
132.         }
     
133.         if( check_rightline() ) {     
     
134.             pattern = 51;
     
135.             break;
     
136.         }
     
137.         if( check_leftline() ) {      
     
138.             pattern = 61;
     
139.             break;
     
140.         }
     
141.         switch( sensor_inp(MASK3_3) ) {
     
142.             case 0x00:
     
143.                
     
144.                 handle( 0 );
     
145.                 speed( 100 ,100 );
     
146.                 break;
     
147.             case 0x04:
     
148.               
     
149.                 handle( 5 );
     
150.                 speed( 100 ,100 );
     
151.                 break;
     
152.             case 0x06:
     
153.          
     
154.                 handle( 10 );
     
155.                 speed( 80 ,67 );
     
156.                 break;
     
157.             case 0x07:
     
158.             
     
159.                 handle( 15 );
     
160.                 speed( 50 ,38 );
     
161.                 break;
     
162.             case 0x03:
     
163.                 handle( 25 );
     
164.                 speed( 30 ,19 );
     
165.                 pattern = 12;
     
166.                 break;
     
167.             case 0x20:
     
168.                 handle( -5 );
     
169.                 speed( 100 ,100 );
     
170.                 break;
     
171.             case 0x60:
     
172.                
     
173.                 handle( -10 );
     
174.                 speed( 67 ,80 );
     
175.                 break;
     
176.             case 0xe0:
     
177.                handle( -15 );
     
178.                 speed( 38 ,50 );
     
179.                 break;
     
180.             case 0xc0:
     
181.                
     
182.                 handle( -25 );
     
183.                 speed( 19 ,30 );
     
184.                 pattern = 13;
     
185.                 break;
     
186.             default:
     
187.                 break;
     
188.         }
     
189.         break;
     
190.     case 12:
     
191.                if( check_crossline() ) {      
     
192.             pattern = 21;
     
193.             break;
     
194.         }
     
195.         if( check_rightline() ) {      
     
196.             pattern = 51;
     
197.             break;
     
198.         }
     
199.         if( check_leftline() ) {      
     
200.             pattern = 61;
     
201.             break;
     
202.         }
     
203.         if( sensor_inp(MASK3_3) == 0x06 ) {
     
204.             pattern = 11;
     
205.         }
     
206.         break;
     
207.     case 13:
     
208.                 if( check_crossline() ) {                pattern = 21;
     
209.             break;
     
210.         }
     
211.         if( check_rightline() ) {      
     
212.             pattern = 51;
     
213.             break;
     
214.         }
     
215.         if( check_leftline() ) {        
     
216.             pattern = 61;
     
217.             break;
     
218.         }
     
219.         if( sensor_inp(MASK3_3) == 0x60 ) {
     
220.             pattern = 11;
     
221.         }
     
222.         break;
     

复制代码

制作爱好者

1.     case 21:
   
2.         
   
3.         led_out( 0x3 );
   
4.         handle( 0 );
   
5.         speed( 0 ,0 );
   
6.         pattern = 22;
   
7.         cnt1 = 0;
   
8.         break;
   
9.     case 22:
   
10.         
    
11.         if( cnt1 > 100 ) {
    
12.             pattern = 23;
    
13.             cnt1 = 0;
    
14.         }
    
15.         break;
    
16.     case 23:
    
17.         
    
18.         if( sensor_inp(MASK4_4)==0xf8 ) {
    
19.             
    
20.             led_out( 0x1 );
    
21.             handle( -38 );
    
22.             speed( 10 ,50 );
    
23.             pattern = 31;
    
24.             cnt1 = 0;
    
25.             break;
    
26.         }
    
27.         if( sensor_inp(MASK4_4)==0x1f ) {
    
28.                         led_out( 0x2 );
    
29.             handle( 38 );
    
30.             speed( 50 ,10 );
    
31.             pattern = 41;
    
32.             cnt1 = 0;
    
33.             break;
    
34.         }
    
35.         switch( sensor_inp(MASK3_3) ) {
    
36.             case 0x00:
    
37.          
    
38.                 handle( 0 );
    
39.                 speed( 40 ,40 );
    
40.                 break;
    
41.             case 0x04:
    
42.             case 0x06:
    
43.             case 0x07:
    
44.             case 0x03:
    
45.               
    
46.                 handle( 8 );
    
47.                 speed( 40 ,35 );
    
48.                 break;
    
49.             case 0x20:
    
50.             case 0x60:
    
51.             case 0xe0:
    
52.             case 0xc0:
    
53.          
    
54.                 handle( -8 );
    
55.                 speed( 35 ,40 );
    
56.                 break;
    
57.         }
    
58.         break;
    
59.     case 31:
    
60.       
    
61.         if( cnt1 > 200 ) {
    
62.             pattern = 32;
    
63.             cnt1 = 0;
    
64.         }
    
65.         break;
    
66.     case 32:
    
67.         
    
68.         if( sensor_inp(MASK3_3) == 0x60 ) {
    
69.             led_out( 0x0 );
    
70.             pattern = 11;
    
71.             cnt1 = 0;
    
72.         }
    
73.         break;
    
74.     case 41:
    
75.              if( cnt1 > 200 ) {
    
76.             pattern = 42;
    
77.             cnt1 = 0;
    
78.         }
    
79.         break;
    
80.     case 42:
    
81.      
    
82.         if( sensor_inp(MASK3_3) == 0x06 ) {
    
83.             led_out( 0x0 );
    
84.             pattern = 11;
    
85.             cnt1 = 0;
    
86.         }
    
87.         break;
    
88.     case 51:
    
89.   
    
90.         led_out( 0x2 );
    
91.         handle( 0 );
    
92.         speed( 0 ,0 );
    
93.         pattern = 52;
    
94.         cnt1 = 0;
    
95.         break;
    
96.     case 52:
    
97.    
    
98.         if( cnt1 > 100 ) {
    
99.             pattern = 53;
    
100.             cnt1 = 0;
     
101.         }
     
102.         break;
     
103.     case 53:
     
104.      
     
105.         if( sensor_inp(MASK4_4) == 0x00 ) {
     
106.             handle( 15 );
     
107.             speed( 40 ,31 );
     
108.             pattern = 54;
     
109.             cnt1 = 0;
     
110.             break;
     
111.         }
     
112.         switch( sensor_inp(MASK3_3) ) {
     
113.             case 0x00:
     
114.            
     
115.                 handle( 0 );
     
116.                 speed( 40 ,40 );
     
117.                 break;
     
118.             case 0x04:
     
119.             case 0x06:
     
120.             case 0x07:
     
121.             case 0x03:
     
122.            
     
123.                 handle( 8 );
     
124.                 speed( 40 ,35 );
     
125.                 break;
     
126.             case 0x20:
     
127.             case 0x60:
     
128.             case 0xe0:
     
129.             case 0xc0:
     
130.          
     
131.                 handle( -8 );
     
132.                 speed( 35 ,40 );
     
133.                 break;
     
134.             default:
     
135.                 break;
     
136.         }
     
137.         break;
     
138.     case 54:
     
139.       
     
140.         if( sensor_inp( MASK4_4 ) == 0x3c ) {
     
141.             led_out( 0x0 );
     
142.             pattern = 11;
     
143.             cnt1 = 0;
     
144.         }
     
145.         break;
     
146.     case 61:
     
147.       
     
148.         led_out( 0x1 );
     
149.         handle( 0 );
     
150.         speed( 0 ,0 );
     
151.         pattern = 62;
     
152.         cnt1 = 0;
     
153.         break;
     
154.     case 62:
     
155.       
     
156.         if( cnt1 > 100 ) {
     
157.             pattern = 63;
     
158.             cnt1 = 0;
     
159.         }
     
160.         break;
     
161.     case 63:
     
162.       
     
163.         if( sensor_inp(MASK4_4) == 0x00 ) {
     
164.             handle( -15 );
     
165.             speed( 31 ,40 );
     
166.             pattern = 64;
     
167.             cnt1 = 0;
     
168.             break;
     
169.         }
     
170.         switch( sensor_inp(MASK3_3) ) {
     
171.             case 0x00:
     
172.             
     
173.                 handle( 0 );
     
174.                 speed( 40 ,40 );
     
175.                 break;
     
176.             case 0x04:
     
177.             case 0x06:
     
178.             case 0x07:
     
179.             case 0x03:
     
180.             
     
181.                 handle( 8 );
     
182.                 speed( 40 ,35 );
     
183.                 break;
     
184.             case 0x20:
     
185.             case 0x60:
     
186.             case 0xe0:
     
187.             case 0xc0:
     
188.               
     
189.                 handle( -8 );
     
190.                 speed( 35 ,40 );
     
191.                 break;
     
192.             default:
     
193.                 break;
     
194.         }
     
195.         break;
     
196.     case 64:
     
197.         
     
198.         if( sensor_inp( MASK4_4 ) == 0x3c ) {
     
199.             led_out( 0x0 );
     
200.             pattern = 11;
     
201.             cnt1 = 0;
     
202.         }
     
203.         break;
     
204.     default:
     
205.       
     
206.         pattern = 0;
     
207.         break;
     
208.     }
     
209.     }
     
210. }
     
211. 
     
212. void init( void )
     
213. {
     
214.    
     
215.     P1DDR = 0xff;
     
216.     P2DDR = 0xff;
     
217.     P3DDR = 0xff;
     
218.     P4DDR = 0xff;
     
219.     P5DDR = 0xff;
     
220.     P6DDR = 0xf0;                       
     
221.     P8DDR = 0xff;
     
222.     P9DDR = 0xf7;                       
     
223.     PADDR = 0xff;
     
224.     PBDR  = 0xc0;
     
225.     PBDDR = 0xfe;                     
     
226. 
     
227.     ITU0_TCR = 0x23;                  
     
228.     ITU0_GRA = F_PWM_CYCLE;           
     
229.     ITU0_GRB = 0;                     
     
230.     ITU0_IER = 0x01;                  
     
231.     ITU1_TCR = 0x23;               
     
232.     ITU1_GRA = F_PWM_CYCLE;            
     
233.     ITU1_GRB = 0;                     
     
234.    
     
235.     ITU3_TCR = 0x23;
     
236.     ITU_FCR  = 0x3e;
     
237.     ITU3_GRA = PWM_CYCLE;           
     
238.     ITU3_GRB = ITU3_BRB = 0;         
     
239.     ITU4_GRA = ITU4_BRA = 0;           
     
240.     ITU4_GRB = ITU4_BRB = SERVO_CENTER;
     
241.     ITU_TOER = 0x38;
     
242. 
     
243.     ITU_MDR = 0x03;                  
     
244.     ITU_STR = 0x1f;                  
     
245. }
     
246. 
     
247. #pragma interrupt( interrupt_timer0 )
     
248. void interrupt_timer0( void )
     
249. {
     
250.     ITU0_TSR &= 0xfe;                 
     
251.     cnt0++;
     
252.     cnt1++;
     
253. }
     
254. 
     
255. void timer( unsigned long timer_set )
     
256. {
     
257.     cnt0 = 0;
     
258.     while( cnt0 < timer_set );
     
259. }
     
260. 
     
261. unsigned char sensor_inp( unsigned char mask )
     
262. {
     
263.     unsigned char sensor;
     
264.     sensor  = ~P7DR;
     
265.     sensor  &= 0xef;
     
266.     if( sensor & 0x08 ) sensor |= 0x10;
     
267.     sensor &= mask;
     
268.     return sensor;
     
269. }
     
270. 
     
271. int check_crossline( void )
     
272. {
     
273.     unsigned char b;
     
274.     int ret;
     
275.     ret = 0;
     
276.     b = sensor_inp(MASK3_3);
     
277.     if( b==0xe7 ) {
     
278.         ret = 1;
     
279.     }
     
280.     return ret;
     
281. }
     
282. 
     
283. int check_rightline( void )
     
284. {
     
285.     unsigned char b;
     
286.     int ret;
     
287.     ret = 0;
     
288.     b = sensor_inp(MASK4_4);
     
289.     if( b==0x1f ) {
     
290.         ret = 1;
     
291.     }
     
292.     return ret;
     
293. }
     
294. 
     
295. int check_leftline( void )
     
296. {
     
297.     unsigned char b;
     
298.     int ret;
     
299.     ret = 0;
     
300.     b = sensor_inp(MASK4_4);
     
301.     if( b==0xf8 ) {
     
302.         ret = 1;
     
303.     }
     
304.     return ret;
     
305. }
     
306. 
     
307. unsigned char dipsw_get( void )
     
308. {
     
309.     unsigned char sw;
     
310.     sw  = ~P6DR;                     
     
311.     return  sw;
     
312. }
     
313. 
     
314. unsigned char pushsw_get( void )
     
315. {
     
316.     unsigned char sw;
     
317.     sw  = ~PBDR;                     
     
318.     sw &= 0x01;
     
319.     return  sw;
     
320. }
     

复制代码

四月份先生

感谢，收下！！

苏小贱

顶一个