标题: 简单机器人0上台防掉台小程序 [打印本页]

---

作者: 漂洋过海    时间: 2017-7-21 15:07
标题: 简单机器人0上台防掉台小程序
机器人大赛，小白参赛

所有资料51hei提供下载:
Debug.zip (1.85 KB, 下载次数: 7)

2017-7-21 15:06 上传

点击文件名下载附件
下载积分: 黑币 -5

0上台防掉台源程序如下:

1. #include  "Apps/SystemTask.h"
   
2. uint8 SERVO_MAPPING[5] = {1,2,3,4,5};
   
3. int main()
   
4. {
   
5.     int  ad[6] = {0};
   
6.     int  io[12] = {0};
   
7.     int  ave = 0;
   
8.     MFInit();
   
9.     MFInitServoMapping(&SERVO_MAPPING[0],5);
   
10.     MFSetPortDirect(0x00000000);
    
11.     MFSetServoMode(1,1);
    
12.     MFSetServoMode(2,1);
    
13.     MFSetServoMode(3,0);
    
14.     MFSetServoMode(4,0);
    
15.     MFSetServoMode(5,0);
    
16.     //初始化状态
    
17.     MFSetServoRotaSpd(1,0);
    
18.     MFSetServoRotaSpd(2,0);
    
19.     MFSetServoPos(3,617,512);
    
20.     MFSetServoPos(4,358,512);
    
21.     MFSetServoPos(5,581,512);
    
22.     MFServoAction();
    
23.     //io1
    
24.     io[1] = MFGetDigiInput(1);
    
25.     while (1)
    
26.     {
    
27.         if (io[1]==0)
    
28.         {
    
29.             break ;
    
30.         }
    
31.         //io1
    
32.         io[1] = MFGetDigiInput(1);
    
33.     }
    
34.     //立爪
    
35.     MFSetServoRotaSpd(1,0);
    
36.     MFSetServoRotaSpd(2,0);
    
37.     MFSetServoPos(3,280,512);
    
38.     MFSetServoPos(4,825,512);
    
39.     MFSetServoPos(5,116,512);
    
40.     MFServoAction();
    
41.     DelayMS(1000);
    
42.     //立爪前进
    
43.     MFSetServoRotaSpd(1,400);
    
44.     MFSetServoRotaSpd(2,-400);
    
45.     MFSetServoPos(3,280,512);
    
46.     MFSetServoPos(4,825,512);
    
47.     MFSetServoPos(5,116,512);
    
48.     MFServoAction();
    
49.     DelayMS(1000);
    
50.     //放前爪
    
51.     MFSetServoRotaSpd(1,400);
    
52.     MFSetServoRotaSpd(2,-400);
    
53.     MFSetServoPos(3,721,512);
    
54.     MFSetServoPos(4,825,512);
    
55.     MFSetServoPos(5,116,512);
    
56.     MFServoAction();
    
57.     DelayMS(1000);
    
58.     //放后爪
    
59.     MFSetServoRotaSpd(1,400);
    
60.     MFSetServoRotaSpd(2,-400);
    
61.     MFSetServoPos(3,280,512);
    
62.     MFSetServoPos(4,300,512);
    
63.     MFSetServoPos(5,649,512);
    
64.     MFServoAction();
    
65.     DelayMS(1000);
    
66.     //台上状态
    
67.     MFSetServoRotaSpd(1,400);
    
68.     MFSetServoRotaSpd(2,-400);
    
69.     MFSetServoPos(3,617,512);
    
70.     MFSetServoPos(4,358,512);
    
71.     MFSetServoPos(5,581,512);
    
72.     MFServoAction();
    
73.     DelayMS(1000);
    
74.     while (1)
    
75.     {
    
76.         //io3
    
77.         io[3] = MFGetDigiInput(3);
    
78.         //io4
    
79.         io[4] = MFGetDigiInput(4);
    
80.         //io5
    
81.         io[5] = MFGetDigiInput(5);
    
82.         //io6
    
83.         io[6] = MFGetDigiInput(6);
    
84.         //台上状态
    
85.         MFSetServoRotaSpd(1,600);
    
86.         MFSetServoRotaSpd(2,-600);
    
87.         MFSetServoPos(3,617,512);
    
88.         MFSetServoPos(4,358,512);
    
89.         MFSetServoPos(5,581,512);
    
90.         MFServoAction();
    
91.         //io3
    
92.         io[3] = MFGetDigiInput(3);
    
93.         if (io[3]==1)
    
94.         {
    
95.             //后退
    
96.             MFSetServoRotaSpd(1,-400);
    
97.             MFSetServoRotaSpd(2,400);
    
98.             MFSetServoPos(3,671,512);
    
99.             MFSetServoPos(4,358,512);
    
100.             MFSetServoPos(5,581,512);
     
101.             MFServoAction();
     
102.             DelayMS(400);
     
103.             //右转
     
104.             MFSetServoRotaSpd(1,700);
     
105.             MFSetServoRotaSpd(2,700);
     
106.             MFSetServoPos(3,671,512);
     
107.             MFSetServoPos(4,358,512);
     
108.             MFSetServoPos(5,581,512);
     
109.             MFServoAction();
     
110.             DelayMS(400);
     
111.             //台上状态
     
112.             MFSetServoRotaSpd(1,400);
     
113.             MFSetServoRotaSpd(2,-400);
     
114.             MFSetServoPos(3,617,512);
     
115.             MFSetServoPos(4,358,512);
     
116.             MFSetServoPos(5,581,512);
     
117.             MFServoAction();
     
118.         }
     
119.         else
     
120.         {
     
121.             //io4
     
122.             io[4] = MFGetDigiInput(4);
     
123.             if (io[4]==1)
     
124.             {
     
125.                 //后退
     
126.                 MFSetServoRotaSpd(1,-400);
     
127.                 MFSetServoRotaSpd(2,400);
     
128.                 MFSetServoPos(3,617,512);
     
129.                 MFSetServoPos(4,358,512);
     
130.                 MFSetServoPos(5,581,512);
     
131.                 MFServoAction();
     
132.                 DelayMS(400);
     
133.                 //左转
     
134.                 MFSetServoRotaSpd(1,-700);
     
135.                 MFSetServoRotaSpd(2,-700);
     
136.                 MFSetServoPos(3,617,512);
     
137.                 MFSetServoPos(4,358,512);
     
138.                 MFSetServoPos(5,581,512);
     
139.                 MFServoAction();
     
140.                 DelayMS(400);
     
141.                 //台上状态
     
142.                 MFSetServoRotaSpd(1,400);
     
143.                 MFSetServoRotaSpd(2,-400);
     
144.                 MFSetServoPos(3,617,512);
     
145.                 MFSetServoPos(4,358,512);
     
146.                 MFSetServoPos(5,581,512);
     
147.                 MFServoAction();
     
148.             }
     
149.             else
     
150.             {
     
151.                 //io5
     
152.                 io[5] = MFGetDigiInput(5);
     
153.                 if (io[5]==1)
     
154.                 {
     
155.                     //前进
     
156.                     MFSetServoRotaSpd(1,400);
     
157.                     MFSetServoRotaSpd(2,-400);
     
158.                     MFSetServoPos(3,617,512);
     
159.                     MFSetServoPos(4,358,512);
     
160.                     MFSetServoPos(5,581,512);
     
161.                     MFServoAction();
     
162.                     DelayMS(400);
     
163.                     //右转
     
164.                     MFSetServoRotaSpd(1,700);
     
165.                     MFSetServoRotaSpd(2,700);
     
166.                     MFSetServoPos(3,617,512);
     
167.                     MFSetServoPos(4,358,512);
     
168.                     MFSetServoPos(5,581,512);
     
169.                     MFServoAction();
     
170.                     DelayMS(200);
     
171.                     //台上状态
     
172.                     MFSetServoRotaSpd(1,400);
     
173.                     MFSetServoRotaSpd(2,-400);
     
174.                     MFSetServoPos(3,617,512);
     
175.                     MFSetServoPos(4,358,512);
     
176.                     MFSetServoPos(5,581,512);
     
177.                     MFServoAction();
     
178.                 }
     
179.                 else
     
180.                 {
     
181.                     //io6
     
182.                     io[6] = MFGetDigiInput(6);
     
183.                     if (io[6]==1)
     
184.                     {
     
185.                         //前进
     
186.                         MFSetServoRotaSpd(1,400);
     
187.                         MFSetServoRotaSpd(2,-400);
     
188.                         MFSetServoPos(3,617,512);
     
189.                         MFSetServoPos(4,358,512);
     
190.                         MFSetServoPos(5,581,512);
     
191.                         MFServoAction();
     
192.                         DelayMS(500);
     
193.                         //左转
     
194.                         MFSetServoRotaSpd(1,-700);
     
195.                         MFSetServoRotaSpd(2,-700);
     
196.                         MFSetServoPos(3,617,512);
     
197.                         MFSetServoPos(4,358,512);
     
198.                         MFSetServoPos(5,581,512);
     
199.                         MFServoAction();
     
200.                         DelayMS(200);
     
201.                         //台上状态
     
202.                         MFSetServoRotaSpd(1,400);
     
203.                         MFSetServoRotaSpd(2,-400);
     
204.                         MFSetServoPos(3,617,512);
     
205.                         MFSetServoPos(4,358,512);
     
206.                         MFSetServoPos(5,581,512);
     
207.                         MFServoAction();
     
208.                     }
     
209.                     else
     
210.                     {
     
211.                         //io3
     
212.                         io[3] = MFGetDigiInput(3);
     
213.                         //io4
     
214.                         io[4] = MFGetDigiInput(4);
     
215.                         if ((io[3]&&io[4]==1))
     
216.                         {
     
217.                             //后退
     
218.                             MFSetServoRotaSpd(1,-400);
     
219.                             MFSetServoRotaSpd(2,400);
     
220.                             MFSetServoPos(3,671,512);
     
221.                             MFSetServoPos(4,358,512);
     
222.                             MFSetServoPos(5,581,512);
     
223.                             MFServoAction();
     
224.                             DelayMS(1000);
     
225.                             //右转
     
226.                             MFSetServoRotaSpd(1,700);
     
227.                             MFSetServoRotaSpd(2,700);
     
228.                             MFSetServoPos(3,617,512);
     
229.                             MFSetServoPos(4,358,512);
     
230.                             MFSetServoPos(5,581,512);
     
231.                             MFServoAction();
     
232.                             DelayMS(200);
     
233.                             //台上状态
     
234.                             MFSetServoRotaSpd(1,400);
     
235.                             MFSetServoRotaSpd(2,-400);
     
236.                             MFSetServoPos(3,617,512);
     
237.                             MFSetServoPos(4,358,512);
     
238.                             MFSetServoPos(5,581,512);
     
239.                             MFServoAction();
     
240.                         }
     
241.                         else
     
242.                         {
     
243.                             //io5
     
244.                             io[5] = MFGetDigiInput(5);
     
245.                             //io6
     
246.                             io[6] = MFGetDigiInput(6);
     
247.                             if ((io[5]&&io[6]==1))
     
248.                             {
     
249.                                 //后退
     
250.                                 MFSetServoRotaSpd(1,-400);
     
251.                                 MFSetServoRotaSpd(2,400);
     
252.                                 MFSetServoPos(3,671,512);
     
253.                                 MFSetServoPos(4,358,512);
     
254.                                 MFSetServoPos(5,581,512);
     
255.                                 MFServoAction();
     
256.                                 DelayMS(500);
     
257.                                 //右转
     
258.                                 MFSetServoRotaSpd(1,700);
     
259.                                 MFSetServoRotaSpd(2,700);
     
260.                                 MFSetServoPos(3,617,512);
     
261.                                 MFSetServoPos(4,358,512);
     
262.                                 MFSetServoPos(5,581,512);
     
263.                                 MFServoAction();
     
264.                                 DelayMS(200);
     
265.                                 //台上状态
     
266.                                 MFSetServoRotaSpd(1,400);
     
267.                                 MFSetServoRotaSpd(2,-400);
     
268.                                 MFSetServoPos(3,617,512);
     
269.                                 MFSetServoPos(4,358,512);
     
270.                                 MFSetServoPos(5,581,512);
     
271.                                 MFServoAction();
     
272.                             }
     
273.                             else
     
274.                             {
     
275.                                 //io3
     
276.                                 io[3] = MFGetDigiInput(3);
     
277.                                 //io5
     
278.                                 io[5] = MFGetDigiInput(5);
     
279.                                 if ((io[3]&&io[5]==1))
     
280.                                 {
     
281.                                     //后退
     
282.                                     MFSetServoRotaSpd(1,-400);
     
283.                                     MFSetServoRotaSpd(2,400);
     
284.                                     MFSetServoPos(3,671,512);
     
285.                                     MFSetServoPos(4,358,512);
     
286.                                     MFSetServoPos(5,581,512);
     
287.                                     MFServoAction();
     
288.                                     DelayMS(200);
     
289.                                     //左转
     
290.                                     MFSetServoRotaSpd(1,-700);
     
291.                                     MFSetServoRotaSpd(2,-700);
     
292.                                     MFSetServoPos(3,617,512);
     
293.                                     MFSetServoPos(4,358,512);
     
294.                                     MFSetServoPos(5,581,512);
     
295.                                     MFServoAction();
     
296.                                     DelayMS(200);
     
297.                                     //台上状态
     
298.                                     MFSetServoRotaSpd(1,400);
     
299.                                     MFSetServoRotaSpd(2,-400);
     
300.                                     MFSetServoPos(3,617,512);
     
301.                                     MFSetServoPos(4,358,512);
     
302.                                     MFSetServoPos(5,581,512);
     
303.                                     MFServoAction();
     
304.                                 }
     
305.                                 else
     
306.                                 {
     
307.                                     //io4
     
308.                                     io[4] = MFGetDigiInput(4);
     
309.                                     //io6
     
310.                                     io[6] = MFGetDigiInput(6);
     
311.                                     if ((io[4]&&io[6]==1))
     
312.                                     {
     
313.                                         //前进
     
314.                                         MFSetServoRotaSpd(1,400);
     
315.                                         MFSetServoRotaSpd(2,-400);
     
316.                                         MFSetServoPos(3,617,512);
     
317.                                         MFSetServoPos(4,358,512);
     
318.                                         MFSetServoPos(5,581,512);
     
319.                                         MFServoAction();
     
320.                                         DelayMS(500);
     
321.                                         //台上状态
     
322.                                         MFSetServoRotaSpd(1,400);
     
323.                                         MFSetServoRotaSpd(2,-400);
     
324.                                         MFSetServoPos(3,617,512);
     
325.                                         MFSetServoPos(4,358,512);
     
326.                                         MFSetServoPos(5,581,512);
     
327.                                         MFServoAction();
     
328.                                     }
     
329.                                     else
     
330.                                     {
     
331.                                         //io3
     
332.                                         io[3] = MFGetDigiInput(3);
     
333.                                         //io4
     
334.                                         io[4] = MFGetDigiInput(4);
     
335.                                         //io5
     
336.                                         io[5] = MFGetDigiInput(5);
     
337.                                         if ((io[3]&&io[4]&&io[5]==1))
     
338.                                         {
     
339.                                             //右转
     
340.                                             MFSetServoRotaSpd(1,700);
     
341.                                             MFSetServoRotaSpd(2,700);
     
342.                                             MFSetServoPos(3,617,512);
     
343.                                             MFSetServoPos(4,358,512);
     
344.                                             MFSetServoPos(5,581,512);
     
345.                                             MFServoAction();
     
346.                                             DelayMS(200);
     
347.                                             //台上状态
     
348.                                             MFSetServoRotaSpd(1,400);
     
349.                                             MFSetServoRotaSpd(2,-400);
     
350.                                             MFSetServoPos(3,617,512);
     
351.                                             MFSetServoPos(4,358,512);
     
352.                                             MFSetServoPos(5,581,512);
     
353.                                             MFServoAction();
     
354.                                         }
     
355.                                         else
     
356.                                         {
     
357.                                             //io4
     
358.                                             io[4] = MFGetDigiInput(4);
     
359.                                             //io5
     
360.                                             io[5] = MFGetDigiInput(5);
     
361.                                             //io6
     
362.                                             io[6] = MFGetDigiInput(6);
     
363.                                             if ((io[4]&&io[5]&&io[6]==1))
     
364.                                             {
     
365.                                                 //左转
     
366.                                                 MFSetServoRotaSpd(1,-700);
     
367.                                                 MFSetServoRotaSpd(2,-700);
     
368.                                                 MFSetServoPos(3,617,512);
     
369.                                                 MFSetServoPos(4,358,512);
     
370.                                                 MFSetServoPos(5,581,512);
     
371.                                                 MFServoAction();
     
372.                                                 DelayMS(200);
     
373.                                                 //台上状态
     
374.                                                 MFSetServoRotaSpd(1,400);
     
375.                                                 MFSetServoRotaSpd(2,-400);
     
376.                                                 MFSetServoPos(3,617,512);
     
377.                                                 MFSetServoPos(4,358,512);
     
378.                                                 MFSetServoPos(5,581,512);
     
379.                                                 MFServoAction();
     
380.                                             }
     
381.                                             else
     
382.                                             {
     
383.                                                 //io1
     
384.                                                 io[1] = MFGetDigiInput(1);
     
385.                                                 if (io[1]==0)
     
386.                                                 {
     
387.                                                     DelayMS(200);
     
388.                                                     //撞击
     
389.                                                     MFSetServoRotaSpd(1,900);
     
390.                                                     MFSetServoRotaSpd(2,-900);
     
391.                                                     MFSetServoPos(3,617,512);
     
392.                                                     MFSetServoPos(4,358,512);
     
393.                                                     MFSetServoPos(5,581,512);
     
394.                                                     MFServoAction();
     
395. ……………………
     
396. 
     
397. …………限于本文篇幅 余下代码请从51黑下载附件…………
     

复制代码

微信图片_20170721150432.png (32.4 KB, 下载次数: 126)

下载附件

2017-7-21 15:05 上传

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1