【转载】用Blynk控制带有M5Atom的小型双足机器人

vany5921

   我使用M5Atom Matrix（这是M5Stack系列中最小的M5Atom系列）制造了一个双足步行机器人。


2.构成组件

1）M5Atom

2）FITEC FS90舵机 x6

3）电池x3


该结构是使用Fusion 360设计的。




3.组装
在组装时，很难推入舵机接线。橡胶粘贴在脚底上以防止打滑。

1. #include "M5Atom.h"
   
2. #include "BluetoothSerial.h" //BluetoothSerial
   
3. 
   
4. BluetoothSerial SerialBT;
   
5. 
   
6. const uint8_t Srv0 = 22; //GPIO Right Arm
   
7. const uint8_t Srv1 = 19; //GPIO Right Leg
   
8. const uint8_t Srv2 = 23; //GPIO Right Foot
   
9. const uint8_t Srv3 = 33; //GPIO Left Foot
   
10. const uint8_t Srv4 = 25; //GPIO Left Leg
    
11. const uint8_t Srv5 = 21; //GPIO Left Arm
    
12. 
    
13. const uint8_t srv_CH0 = 0, srv_CH1 = 1, srv_CH2 = 2, srv_CH3 = 3, srv_CH4 = 4, srv_CH5 = 5; //チャンネル
    
14. const double PWM_Hz = 50;   //PWM频率
    
15. const uint8_t PWM_level = 16; //PWM 16bit(0～65535)
    
16. 
    
17. int pulseMIN = 1640;  //0deg 500μsec 50Hz 16bit
    
18. int pulseMAX = 8190;  //180deg 2500μsec 50Hz 16bit
    
19. 
    
20. int cont_min = 0;
    
21. int cont_max = 180;
    
22. 
    
23. int angZero[] = {90, 78, 85, 88, 92, 90};
    
24. int ang0[6];
    
25. int ang1[6];
    
26. int ang_b[6];
    
27. char ang_c[6];
    
28. int ts=300;
    
29. int td=20;
    
30. 
    
31. bool IMU6886Flag = false;  //
    
32. 
    
33. // Forward Step
    
34. int f_s[19][6]={
    
35.   {0,0,0,0,0,0},
    
36.   {0,0,-15,-10,0,0},
    
37.   {0,0,-15,-15,0,0},
    
38.   {-20,15,-15,-15,15,-20},
    
39.   {-20,15,0,0,15,-20},
    
40.   {-20,15,10,15,15,-20},
    
41.   {-20,15,15,15,15,-20},
    
42.   {0,0,15,15,0,0},
    
43.   {20,-15,15,15,-15,20},
    
44.   {20,-15,0,0,-15,20},
    
45.   {20,-15,-15,-10,-15,20},
    
46.   {20,-15,-15,-15,-15,20},
    
47.   {0,0,-15,-15,0,0},
    
48.   {-20,15,-15,-15,15,-20},
    
49.   {-20,15,0,0,15,-20},
    
50.   {-20,15,10,15,15,-20},
    
51.   {-20,15,15,15,15,-20},
    
52.   {0,0,15,15,0,0}};
    
53. 
    
54. int b_s[19][6]={
    
55.   {0,0,0,0,0,0},
    
56.   {0,0,-15,-10,0,0},
    
57.   {0,0,-15,-15,0,0},
    
58.   {20,-15,-15,-15,-15,20},
    
59.   {20,-15,0,0,-15,20},
    
60.   {20,-15,10,15,-15,20},
    
61.   {20,-15,15,15,-15,20},
    
62.   {0,0,15,15,0,0},
    
63.   {-20,15,15,15,15,-20},
    
64.   {-20,15,0,0,15,-20},
    
65.   {-20,15,-15,-10,15,-20},
    
66.   {-20,15,-15,-15,15,-20},
    
67.   {0,0,-15,-15,0,0},
    
68.   {20,-15,-15,-15,-15,20},
    
69.   {20,-15,0,0,-15,20},
    
70.   {20,-15,10,15,-15,20},
    
71.   {20,-15,15,15,-15,20},
    
72.   {0,0,15,15,0,0}};
    
73. 
    
74. int l_s[9][6]={
    
75.   {0,0,0,0,0,0},
    
76.   {0,0,10,15,0,0},
    
77.   {0,0,15,15,0,0},
    
78.   {-20,15,15,15,-15,-20},
    
79.   {-20,15,0,0,-15,-20},
    
80.   {-20,15,-15,-10,-15,-20},
    
81.   {-20,15,-15,-15,-15,-20},
    
82.   {0,0,-15,-15,0,0},
    
83.   {0,0,0,0,0,0}};
    
84. 
    
85. int r_s[9][6]={
    
86.   {0,0,0,0,0,0},
    
87.   {0,0,-15,-10,0,0},
    
88.   {0,0,-15,-15,0,0},
    
89.   {-20,15,-15,-15,-15,-20},
    
90.   {-20,15,0,0,-15,-20},
    
91.   {-20,15,10,15,-15,-20},
    
92.   {-20,15,15,15,-15,-20},
    
93.   {0,0,15,15,0,0},
    
94.   {0,0,0,0,0,0}};
    
95. 
    
96. int r_a[7][6]={
    
97.   {0,0,0,0,0,0},
    
98.   {80,0,0,0,0,0},
    
99.   {0,0,0,0,0,0},
    
100.   {80,0,0,0,0,0},
     
101.   {0,0,0,0,0,0},
     
102.   {80,0,0,0,0,0},
     
103.   {0,0,0,0,0,0}};
     
104. 
     
105. int l_a[7][6]={
     
106.   {0,0,0,0,0,0},
     
107.   {0,0,0,0,0,-80},
     
108.   {0,0,0,0,0,0},
     
109.   {0,0,0,0,0,-80},
     
110.   {0,0,0,0,0,0},
     
111.   {0,0,0,0,0,-80},
     
112.   {0,0,0,0,0,0}};
     
113. 
     
114. int delection = 0;
     
115. 
     
116. void Initial_Value(){  //initial servo angle
     
117.   for (int j=0; j <=5 ; j++){
     
118.       ang0[j] = angZero[j];
     
119.   }
     
120.   for (int j=0; j <=5 ; j++){
     
121.       ang1[j] = angZero[j];
     
122.   }
     
123.   servo_set();
     
124. }
     
125. 
     
126. void face_clear(){
     
127.   for(int i=0; i<25; i++){
     
128.     M5.dis.drawpix(i, 0x000000); //black
     
129.     //M5.dis.drawpix(i, 0xa5ff00); //orange
     
130.   }
     
131. }
     
132. 
     
133. void face_center(){
     
134.   M5.dis.drawpix(6, 0x00ff00);  //red
     
135.   M5.dis.drawpix(7, 0x00ff00);
     
136.   M5.dis.drawpix(8, 0x00ff00);
     
137.   M5.dis.drawpix(16, 0x0000ff);  //blue 0x0000ff
     
138.   M5.dis.drawpix(18, 0x0000ff);
     
139. }
     
140. 
     
141. void Srv_drive(int srv_CH,int SrvAng){
     
142.   SrvAng = map(SrvAng, cont_min, cont_max, pulseMIN, pulseMAX);
     
143.   ledcWrite(srv_CH, SrvAng);
     
144. }
     
145. 
     
146. void rs_gets_3c(char gtc[4])
     
147. {
     
148.   char gc;
     
149.   int i;
     
150. 
     
151.     sprintf(gtc,"    ");
     
152.     for( i = 0 ; i < 4 ; i++ )
     
153.     {
     
154.       gc = SerialBT.read();
     
155.       if (gc == ',')
     
156.         break;
     
157.       else
     
158.         gtc[i] = gc;
     
159.     }
     
160. }
     
161. 
     
162. void forward_step()
     
163. {
     
164.   for (int i=0; i <=18 ; i++){
     
165.     for (int j=0; j <=5 ; j++){
     
166.       ang1[j] = angZero[j] + f_s[i][j];
     
167.     }
     
168.   servo_set();
     
169.   }
     
170. }
     
171. 
     
172. void back_step()
     
173. {
     
174.   for (int i=0; i <=18 ; i++){
     
175.     for (int j=0; j <=5 ; j++){
     
176.       ang1[j] = angZero[j] + b_s[i][j];
     
177.     }
     
178.   servo_set();
     
179.   }
     
180. }
     
181. 
     
182. void right_step()
     
183. {
     
184.   for (int i=0; i <=8 ; i++){
     
185.     for (int j=0; j <=5 ; j++){
     
186.       ang1[j] = angZero[j] + r_s[i][j];
     
187.     }
     
188.   servo_set();
     
189.   }
     
190. }
     
191. 
     
192. void left_step()
     
193. {
     
194.   for (int i=0; i <=8 ; i++){
     
195.     for (int j=0; j <=5 ; j++){
     
196.       ang1[j] = angZero[j] + l_s[i][j];
     
197.     }
     
198.   servo_set();
     
199.   }
     
200. }
     
201. 
     
202. void right_arm()
     
203. {
     
204.   for (int i=0; i <=6 ; i++){
     
205.     for (int j=0; j <=5 ; j++){
     
206.       ang1[j] = angZero[j] + r_a[i][j];
     
207.     }
     
208.   servo_set();
     
209.   }
     
210. }
     
211. 
     
212. void left_arm()
     
213. {
     
214.   for (int i=0; i <=6 ; i++){
     
215.     for (int j=0; j <=5 ; j++){
     
216.       ang1[j] = angZero[j] + l_a[i][j];
     
217.     }
     
218.   servo_set();
     
219.   }
     
220. }
     
221. 
     
222. void servo_set(){
     
223.   int a[6],b[6];
     
224.   
     
225.   for (int j=0; j <=5 ; j++){
     
226.       a[j] = ang1[j] - ang0[j];
     
227.       b[j] = ang0[j];
     
228.       ang0[j] = ang1[j];
     
229.   }
     
230. 
     
231.   for (int k=0; k <=td ; k++){
     
232. 
     
233.       Srv_drive(srv_CH0, a[0]*k/td+b[0]);
     
234.       Srv_drive(srv_CH1, a[1]*k/td+b[1]);
     
235.       Srv_drive(srv_CH2, a[2]*k/td+b[2]);
     
236.       Srv_drive(srv_CH3, a[3]*k/td+b[3]);
     
237.       Srv_drive(srv_CH4, a[4]*k/td+b[4]);
     
238.       Srv_drive(srv_CH5, a[5]*k/td+b[5]);
     
239. 
     
240.       delay(ts/td);
     
241.   }
     
242. }
     
243. 
     
244. void setup() {
     
245.   // void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
     
246.   M5.begin(true, false, true);
     
247.   Serial.begin(151200);
     
248.   SerialBT.begin("M5Atom");
     
249. 
     
250.   pinMode(Srv0, OUTPUT);
     
251.   pinMode(Srv1, OUTPUT);
     
252.   pinMode(Srv2, OUTPUT);
     
253.   pinMode(Srv3, OUTPUT);
     
254.   pinMode(Srv4, OUTPUT);
     
255.   pinMode(Srv5, OUTPUT);
     
256.   
     
257.   //电机PWM通道和频率设置
     
258.   ledcSetup(srv_CH0, PWM_Hz, PWM_level);
     
259.   ledcSetup(srv_CH1, PWM_Hz, PWM_level);
     
260.   ledcSetup(srv_CH2, PWM_Hz, PWM_level);
     
261.   ledcSetup(srv_CH3, PWM_Hz, PWM_level);
     
262.   ledcSetup(srv_CH4, PWM_Hz, PWM_level);
     
263.   ledcSetup(srv_CH5, PWM_Hz, PWM_level);
     
264. 
     
265.   //电机引脚和通道设置
     
266.   ledcAttachPin(Srv0, srv_CH0);
     
267.   ledcAttachPin(Srv1, srv_CH1);
     
268.   ledcAttachPin(Srv2, srv_CH2);
     
269.   ledcAttachPin(Srv3, srv_CH3);
     
270.   ledcAttachPin(Srv4, srv_CH4);
     
271.   ledcAttachPin(Srv5, srv_CH5);
     
272. 
     
273.   face_clear();
     
274.   face_center();
     
275. 
     
276.   Initial_Value();
     
277. }
     
278. 
     
279. void loop() {
     
280.   M5.update();
     
281.   if ( M5.Btn.wasReleased() ) {
     
282.     Initial_Value();
     
283.   }
     
284.   
     
285.   if(SerialBT.available()){
     
286.     delection = SerialBT.read();
     
287.     switch (delection) {
     
288.       case 70: // F FWD
     
289.         SerialBT.println("FWD") ;
     
290.         forward_step();
     
291.       break;
     
292.       case 76: // L LEFT
     
293.         SerialBT.println("LEFT") ;
     
294.         left_step();
     
295.       break;
     
296.       case 82: // R Right
     
297.         SerialBT.println("RIGHT") ;
     
298.         right_step();
     
299.       break;
     
300.       case 66: // B Back
     
301.         SerialBT.println("BACK") ;
     
302.         back_step();
     
303.       break;
     
304.       case 72: // H Left Arm
     
305.         SerialBT.println("LEFT_Arm") ;
     
306.         left_arm();
     
307.       break;
     
308.       case 77: // M Right Arm
     
309.         SerialBT.println("RIGHT_Arm") ;
     
310.         right_arm();
     
311.       break;
     
312.       case 68: //Data
     
313.         SerialBT.println("DATA") ;
     
314.         for (int i=0; i <=5; i++){
     
315.           rs_gets_3c(ang_c);
     
316.           ang_b[i] = atoi(ang_c);
     
317.           if ((ang_b[i] >= 0) && (ang_b[i] <=180))
     
318.             {
     
319.               ang0[i] = ang1[i];
     
320.               ang1[i] = ang_b[i];
     
321.             }
     
322.         }
     
323.         servo_set();
     
324.       break;
     
325.     }
     
326.   }
     
327. }

复制代码

现在我已经创建了动作，但是这次我尝试使用Blynk在iPhone上对其进行控制

Blynk按钮控制前进，后退，右转，左转，右手向上和左手向上。

1. #define BLYNK_PRINT Serial
   
2. #define BLYNK_USE_DIRECT_CONNECT
   
3. 
   
4. #include "M5Atom.h"
   
5. #include <BlynkSimpleEsp32_BLE.h>
   
6. #include <BLEDevice.h>
   
7. #include <BLEServer.h>
   
8. 
   
9. char auth[] = "XXXXXXXXXXXXXXXXXXXXXXXX";  //通过电子邮件发送的身份验证令牌
   
10. const uint8_t Srv0 = 22; //GPIO Right Arm
    
11. const uint8_t Srv1 = 19; //GPIO Right Leg
    
12. const uint8_t Srv2 = 23; //GPIO Right Foot
    
13. const uint8_t Srv3 = 33; //GPIO Left Foot
    
14. const uint8_t Srv4 = 25; //GPIO Left Leg
    
15. const uint8_t Srv5 = 21; //GPIO Left Arm
    
16. 
    
17. const uint8_t srv_CH0 = 0, srv_CH1 = 1, srv_CH2 = 2, srv_CH3 = 3, srv_CH4 = 4, srv_CH5 = 5; //チャンネル
    
18. const double PWM_Hz = 50;   //PWM频率
    
19. const uint8_t PWM_level = 16; //PWM 16bit(0～65535)
    
20. 
    
21. int pulseMIN = 1640;  //0deg 500μsec 50Hz 16bit
    
22. int pulseMAX = 8190;  //180deg 2500μsec 50Hz 16bit
    
23. 
    
24. int cont_min = 0;
    
25. int cont_max = 180;
    
26. 
    
27. int angZero[] = {90, 78, 85, 88, 92, 90};
    
28. int ang0[6];
    
29. int ang1[6];
    
30. int ang_b[6];
    
31. char ang_c[6];
    
32. int ts=160;
    
33. int td=20;
    
34. 
    
35. bool IMU6886Flag = false;
    
36. 
    
37. // Forward Step
    
38. int f_s[19][6]={
    
39.   {0,0,0,0,0,0},
    
40.   {0,0,-15,-10,0,0},
    
41.   {0,0,-15,-15,0,0},
    
42.   {-20,15,-15,-15,15,-20},
    
43.   {-20,15,0,0,15,-20},
    
44.   {-20,15,10,15,15,-20},
    
45.   {-20,15,15,15,15,-20},
    
46.   {0,0,15,15,0,0},
    
47.   {20,-15,15,15,-15,20},
    
48.   {20,-15,0,0,-15,20},
    
49.   {20,-15,-15,-10,-15,20},
    
50.   {20,-15,-15,-15,-15,20},
    
51.   {0,0,-15,-15,0,0},
    
52.   {-20,15,-15,-15,15,-20},
    
53.   {-20,15,0,0,15,-20},
    
54.   {-20,15,10,15,15,-20},
    
55.   {-20,15,15,15,15,-20},
    
56.   {0,0,15,15,0,0},
    
57.   {0,0,0,0,0,0}};
    
58. 
    
59. int b_s[19][6]={
    
60.   {0,0,0,0,0,0},
    
61.   {0,0,-20,-15,0,0},
    
62.   {0,0,-15,-15,0,0},
    
63.   {0,-15,-15,-15,-15,20},
    
64.   {0,-15,0,0,-15,20},
    
65.   {0,-15,15,20,-15,20},
    
66.   {0,-15,15,15,-15,20},
    
67.   {0,0,15,15,0,0},
    
68.   {0,15,15,15,15,-20},
    
69.   {0,15,0,0,15,-20},
    
70.   {0,15,-20,-15,15,-20},
    
71.   {0,15,-15,-15,15,-20},
    
72.   {0,0,-15,-15,0,0},
    
73.   {0,-15,-15,-15,-15,20},
    
74.   {0,-15,0,0,-15,20},
    
75.   {0,-15,15,20,-15,20},
    
76.   {0,-15,15,15,-15,20},
    
77.   {0,0,15,15,0,0},
    
78.   {0,0,0,0,0,0}};
    
79. 
    
80. int l_s[9][6]={
    
81.   {0,0,0,0,0,0},
    
82.   {0,0,10,15,0,0},
    
83.   {0,0,15,15,0,0},
    
84.   {-20,15,15,15,-15,-20},
    
85.   {-20,15,0,0,-15,-20},
    
86.   {-20,15,-15,-10,-15,-20},
    
87.   {-20,15,-15,-15,-15,-20},
    
88.   {0,0,-15,-15,0,0},
    
89.   {0,0,0,0,0,0}};
    
90. 
    
91. int r_s[9][6]={
    
92.   {0,0,0,0,0,0},
    
93.   {0,0,-15,-10,0,0},
    
94.   {0,0,-15,-15,0,0},
    
95.   {-20,15,-15,-15,-15,-20},
    
96.   {-20,15,0,0,-15,-20},
    
97.   {-20,15,10,15,-15,-20},
    
98.   {-20,15,15,15,-15,-20},
    
99.   {0,0,15,15,0,0},
    
100.   {0,0,0,0,0,0}};
     
101. 
     
102. int r_a[7][6]={
     
103.   {0,0,0,0,0,0},
     
104.   {80,0,0,0,0,0},
     
105.   {0,0,0,0,0,0},
     
106.   {80,0,0,0,0,0},
     
107.   {0,0,0,0,0,0},
     
108.   {80,0,0,0,0,0},
     
109.   {0,0,0,0,0,0}};
     
110. 
     
111. int l_a[7][6]={
     
112.   {0,0,0,0,0,0},
     
113.   {0,0,0,0,0,-80},
     
114.   {0,0,0,0,0,0},
     
115.   {0,0,0,0,0,-80},
     
116.   {0,0,0,0,0,0},
     
117.   {0,0,0,0,0,-80},
     
118.   {0,0,0,0,0,0}};
     
119. 
     
120. int delection = 0;
     
121. 
     
122. void Initial_Value(){  //initial servo angle
     
123.   for (int j=0; j <=5 ; j++){
     
124.       ang0[j] = angZero[j];
     
125.   }
     
126.   for (int j=0; j <=5 ; j++){
     
127.       ang1[j] = angZero[j];
     
128.   }
     
129.   servo_set();
     
130. }
     
131. 
     
132. void face_clear(){
     
133.   for(int i=0; i<25; i++){
     
134.     M5.dis.drawpix(i, 0x000000); //black
     
135.     //M5.dis.drawpix(i, 0xa5ff00); //orange
     
136.   }
     
137. }
     
138. 
     
139. void face_center(){
     
140.   M5.dis.drawpix(6, 0x00ff00);  //red
     
141.   M5.dis.drawpix(7, 0x00ff00);
     
142.   M5.dis.drawpix(8, 0x00ff00);
     
143.   M5.dis.drawpix(16, 0x0000ff);  //blue 0x0000ff
     
144.   M5.dis.drawpix(18, 0x0000ff);
     
145. }
     
146. 
     
147. void face_right(){
     
148.   face_clear();
     
149.   M5.dis.drawpix(7, 0x00ff00);
     
150.   M5.dis.drawpix(8, 0x00ff00);
     
151.   M5.dis.drawpix(9, 0x00ff00);
     
152.   M5.dis.drawpix(17, 0x0000ff);
     
153.   M5.dis.drawpix(19, 0x0000ff);
     
154. }
     
155. 
     
156. void face_left(){
     
157.   face_clear();
     
158.   M5.dis.drawpix(5, 0x00ff00);
     
159.   M5.dis.drawpix(6, 0x00ff00);
     
160.   M5.dis.drawpix(7, 0x00ff00);
     
161.   M5.dis.drawpix(15, 0x0000ff);
     
162.   M5.dis.drawpix(17, 0x0000ff);
     
163. }
     
164. 
     
165. void Srv_drive(int srv_CH,int SrvAng){
     
166.   SrvAng = map(SrvAng, cont_min, cont_max, pulseMIN, pulseMAX);
     
167.   ledcWrite(srv_CH, SrvAng);
     
168. }
     
169. 
     
170. void forward_step()
     
171. {
     
172.   for (int i=0; i <=18 ; i++){
     
173.     for (int j=0; j <=5 ; j++){
     
174.       ang1[j] = angZero[j] + f_s[i][j];
     
175.     }
     
176.   servo_set();
     
177.   }
     
178.   delection = 0;
     
179. }
     
180. 
     
181. void back_step()
     
182. {
     
183.   for (int i=0; i <=18 ; i++){
     
184.     for (int j=0; j <=5 ; j++){
     
185.       ang1[j] = angZero[j] + b_s[i][j];
     
186.     }
     
187.   servo_set();
     
188.   }
     
189.   delection = 0;
     
190. }
     
191. 
     
192. void right_step()
     
193. {
     
194.   face_right();
     
195.   for (int i=0; i <=8 ; i++){
     
196.     for (int j=0; j <=5 ; j++){
     
197.       ang1[j] = angZero[j] + r_s[i][j];
     
198.     }
     
199.   servo_set();
     
200.   }
     
201.   face_clear();
     
202.   face_center();
     
203.   delection = 0;
     
204. }
     
205. 
     
206. void left_step()
     
207. {
     
208.   face_left();
     
209.   for (int i=0; i <=8 ; i++){
     
210.     for (int j=0; j <=5 ; j++){
     
211.       ang1[j] = angZero[j] + l_s[i][j];
     
212.     }
     
213.   servo_set();
     
214.   }
     
215.   face_clear();
     
216.   face_center();
     
217.   delection = 0;
     
218. }
     
219. 
     
220. void right_arm()
     
221. {
     
222.   face_right();
     
223.   for (int i=0; i <=6 ; i++){
     
224.     for (int j=0; j <=5 ; j++){
     
225.       ang1[j] = angZero[j] + r_a[i][j];
     
226.     }
     
227.   servo_set();
     
228.   }
     
229.   face_clear();
     
230.   face_center();
     
231.   delection = 0;
     
232. }
     
233. 
     
234. void left_arm()
     
235. {
     
236.   face_left();
     
237.   for (int i=0; i <=6 ; i++){
     
238.     for (int j=0; j <=5 ; j++){
     
239.       ang1[j] = angZero[j] + l_a[i][j];
     
240.     }
     
241.   servo_set();
     
242.   }
     
243.   face_clear();
     
244.   face_center();
     
245.   delection = 0;
     
246. }
     
247. 
     
248. void servo_set(){
     
249.   int a[6],b[6];
     
250.   
     
251.   for (int j=0; j <=5 ; j++){
     
252.       a[j] = ang1[j] - ang0[j];
     
253.       b[j] = ang0[j];
     
254.       ang0[j] = ang1[j];
     
255.   }
     
256. 
     
257.   for (int k=0; k <=td ; k++){
     
258. 
     
259.       Srv_drive(srv_CH0, a[0]*k/td+b[0]);
     
260.       Srv_drive(srv_CH1, a[1]*k/td+b[1]);
     
261.       Srv_drive(srv_CH2, a[2]*k/td+b[2]);
     
262.       Srv_drive(srv_CH3, a[3]*k/td+b[3]);
     
263.       Srv_drive(srv_CH4, a[4]*k/td+b[4]);
     
264.       Srv_drive(srv_CH5, a[5]*k/td+b[5]);
     
265. 
     
266.       delay(ts/td);
     
267.   }
     
268. }
     
269. 
     
270. BLYNK_WRITE(V0) {
     
271.   int x = param.asInt();
     
272.   if(x == 1){
     
273.     delection = 70;  //forward step
     
274.     Serial.println("FWD");
     
275.   }
     
276. }
     
277. 
     
278. BLYNK_WRITE(V1) {
     
279.   int x = param.asInt();
     
280.   if(x == 1){
     
281.     delection = 66;  //Back step
     
282.     Serial.println("BACK");
     
283.   }
     
284. }
     
285. 
     
286. BLYNK_WRITE(V2) {
     
287.   int x = param.asInt();
     
288.   if(x == 1){
     
289.     delection = 82;  //Right step
     
290.     Serial.println("RIGHT STEP");
     
291.   }
     
292. }
     
293. 
     
294. BLYNK_WRITE(V3) {
     
295.   int x = param.asInt();
     
296.   if(x == 1){
     
297.     delection = 76;  //Left step
     
298.     Serial.println("LEFT STEP");
     
299.   }
     
300. }
     
301. 
     
302. BLYNK_WRITE(V4) {
     
303.   int x = param.asInt();
     
304.   if(x == 1){
     
305.     delection = 77;  //Right Arm
     
306.     Serial.println("RIGHT ARM");
     
307.   }
     
308. }
     
309. 
     
310. BLYNK_WRITE(V5) {
     
311.   int x = param.asInt();
     
312.   if(x == 1){
     
313.     delection = 72;  //Left Arm
     
314.   Serial.println("LEFT ARM");  
     
315.   }
     
316. }
     
317. 
     
318. void setup() {
     
319.   Serial.begin(151200);
     
320.   // void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
     
321.   M5.begin(true, false, true);
     
322. 
     
323.   Blynk.setDeviceName("Blynk");
     
324.   Blynk.begin(auth);
     
325. 
     
326.   pinMode(Srv0, OUTPUT);
     
327.   pinMode(Srv1, OUTPUT);
     
328.   pinMode(Srv2, OUTPUT);
     
329.   pinMode(Srv3, OUTPUT);
     
330.   pinMode(Srv4, OUTPUT);
     
331.   pinMode(Srv5, OUTPUT);
     
332.   
     
333.   //电机PWM通道和频率设置
     
334.   ledcSetup(srv_CH0, PWM_Hz, PWM_level);
     
335.   ledcSetup(srv_CH1, PWM_Hz, PWM_level);
     
336.   ledcSetup(srv_CH2, PWM_Hz, PWM_level);
     
337.   ledcSetup(srv_CH3, PWM_Hz, PWM_level);
     
338.   ledcSetup(srv_CH4, PWM_Hz, PWM_level);
     
339.   ledcSetup(srv_CH5, PWM_Hz, PWM_level);
     
340. 
     
341.   //电机引脚和通道设置
     
342.   ledcAttachPin(Srv0, srv_CH0);
     
343.   ledcAttachPin(Srv1, srv_CH1);
     
344.   ledcAttachPin(Srv2, srv_CH2);
     
345.   ledcAttachPin(Srv3, srv_CH3);
     
346.   ledcAttachPin(Srv4, srv_CH4);
     
347.   ledcAttachPin(Srv5, srv_CH5);
     
348. 
     
349.   face_center();
     
350. 
     
351.   Initial_Value();
     
352. }
     
353. 
     
354. void loop() {
     
355.   M5.update();
     
356.   Blynk.run();
     
357.   if ( M5.Btn.wasReleased() ) {
     
358.     Initial_Value();
     
359.   }
     
360.   
     
361.   switch (delection) {
     
362.     case 70: // F FWD
     
363.       forward_step();
     
364.     break;
     
365.     case 76: // L LEFT
     
366.       left_step();
     
367.     break;
     
368.     case 82: // R Right
     
369.       right_step();
     
370.     break;
     
371.     case 66: // B Back
     
372.       back_step();
     
373.     break;
     
374.     case 72: // H Left Arm
     
375.       left_arm();
     
376.     break;
     
377.     case 77: // M Right Arm
     
378.       right_arm();
     
379.     break;
     
380.   }
     
381. }

复制代码