机械手动作组编号转换成了角度。。。程序复杂化，求教啊

咸鱼geek

动作组就是六个舵机中的其中几个轮流步进5度来实现，看着像同时运行一样。。。
se，v分别是舵机初始值和待输入的舵机值。。。a，b数组就是舵机执行动作必须改变的舵机角度数组

1. #include <Servo.h>
   
2. //定义变量
   
3. Servo servo[6];
   
4. int se[6]={80,110,80,90,100,130};//舵机初始化数组
   
5. int i=0,t=1000;//t为延时
   
6. 
   
7. String data="";
   
8. int mark=0,v[6]={80,110,80,90,100,130}, over=0;
   
9. boolean tag[6]={1,1,1,1,1,1};//初始化标志位数组,不相等
   
10. 
    
11. //舵机初始化
    
12. void datadeal()
    
13. {
    
14. int j = 0;
    
15. while(Serial.available() > 0)
    
16. {
    
17.   delay(20);
    
18.   data+=char(Serial.read());
    
19.   mark=1;
    
20. }
    
21. if(mark==1)
    
22. {
    
23. Serial.println(data);
    
24. Serial.println(data.length());
    
25.   int A[6]={0};
    
26. for(int i = 0; i < data.length() ; i++)
    
27. {
    
28. 
    
29.    if(data[i] == ',')
    
30.       {
    
31.         j++;
    
32.       }
    
33.        else
    
34.      {
    
35.      A[j] = A[j] * 10 + (data[i] - '0');
    
36.      v[j] =A[j];
    
37.   // v[j] =data[i]*10+(data[i] - '0');
    
38.      }
    
39.   }
    
40. data=String("");
    
41. for(int i = 0; i < 6; i++)
    
42.     {
    
43.       Serial.print("Servo");
    
44.       Serial.print(i);
    
45.       Serial.print(" = ");
    
46.        Serial.println(v[i]);
    
47. 
    
48.     }
    
49.     mark = 0;
    
50.     over=1;
    
51. }
    
52. }
    
53. 
    
54. 
    
55. //单个舵机控制函数
    
56. void fun(int a,int b,int i)
    
57. {
    
58. if(a!=b)
    
59. {
    
60.   if(a<b)
    
61.   {
    
62.   while(a<b)
    
63.        {
    
64.          a+=5;
    
65.      servo[i].write(a);
    
66.      se[i]=a;
    
67.      Serial.println(a);
    
68.      delay(t);
    
69.    
    
70.        }
    
71.   }
    
72. else if(a>b)
    
73.   {
    
74.    while(a>b) {
    
75.        a-=5;
    
76.        servo[i].write(a);
    
77.        se[i]=a;
    
78.      Serial.println(a);
    
79.      delay(t);
    
80.    
    
81.      }
    
82.   }
    
83. }
    
84. 
    
85. else
    
86. return;
    
87. }
    
88. //舵机动作函数
    
89. void Action(int a,int b,int i)
    
90. {
    
91.   if(a<b)
    
92.   {
    
93.          a+=5;
    
94.      servo[i].write(a);
    
95.      se[i]=a;
    
96.      Serial.println(a);
    
97.      delay(t);      
    
98.   }
    
99. else if(a>b)
    
100.   {
     
101.        a-=5;
     
102.        servo[i].write(a);
     
103.        se[i]=a;
     
104.      Serial.println(a);
     
105.      delay(t);     
     
106.    }
     
107. }
     
108. 
     
109. void setup()
     
110. {
     
111.   servo[0].attach(22,380,1200);
     
112.   servo[1].attach(23,380,1200);
     
113.   servo[2].attach(24,380,1200);
     
114.   servo[3].attach(25,380,1200);
     
115.   servo[4].attach(26,380,1200);
     
116.   servo[5].attach(27,380,1200);
     
117.   for(i=0;i<6;i++)
     
118.   servo[i].write(se[i]);
     
119.     delay(2000);
     
120.     Serial.begin(9600);//设置波特率
     
121.     Serial.println("Ready");
     
122. }
     
123. void loop()
     
124. {
     
125. datadeal();
     
126. //单个舵机控制函数
     
127. if(over==1)
     
128. {
     
129. 
     
130. //boolean tag[6]={1,1,1,1,1,1};//初始化标志位数组,不相等
     
131. //判断两个数组元素是否相等
     
132. for(int i=0;i<6;i++)
     
133. {
     
134. //相等则置标志位为0
     
135. if(se[i]==v[i])//对应元素相等则tag=0
     
136. tag[i]=0;
     
137. 
     
138. }
     
139. int sum=0;
     
140. for(int j=0;j<6;j++)
     
141. sum+=tag[j];
     
142. if(sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
     
143. {
     
144. for(int num=0;num<6;num++)
     
145. {
     
146.   Serial.println(v[num]);
     
147. fun(se[num],v[num],num);//函数调用6次
     
148. tag[num]=0;
     
149. }
     
150. }
     
151. //}
     
152. over=0;
     
153. //舵机连续动作组控制
     
154. while(Serial.available() > 0)
     
155. {
     
156.   delay(20);
     
157.   char action=Serial.read();
     
158. 
     
159. int a[6]={80,175,80,90,100,130};//下降
     
160. switch(action)
     
161. {
     
162. case 'a'://下降
     
163. 
     
164. while(sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
     
165. {
     
166. for(int num=0;num<6;num++)
     
167. {
     
168.   Serial.println(v[num]);
     
169.   delay (100);
     
170. Action(se[num],a[num],num);//函数调用6次
     
171. //tag[num]=0;
     
172. delay(50);
     
173. }
     
174. for(int i=0;i<6;i++)
     
175. {
     
176. //相等则置标志位为0
     
177. if(se[i]==v[i])//对应元素相等则tag=0
     
178. tag[i]=0;
     
179. }
     
180. int sum=0;
     
181. for(int j=0;j<6;j++)
     
182. sum+=tag[j];
     
183. if(sum==0)
     
184. break;
     
185. }
     
186. break;
     
187. case '2':
     
188. break;
     
189. //Action();
     
190. }
     
191. }
     
192. }
     
193. }
     

复制代码

回复地方

不懂，帮顶