หน้าแรก > Arduino ESP32

Arduino ESP32

Tags: arduino, esp32, coding, programimg, robot, arm robot, iot, google sheet, wifi, bluetooth, plc, scada, modbus

การประยุกต์ใช้งาน coding Arduino และ ESP32

ดูตัวอย่างและโหลดโค้ดได้ตามลิงค์ ในช่อง Youtube 

Youtube: https://www.youtube.com/@traningteam2022   

  1. - เนื้อหา หัวข้อประมาณนี้ 
    •         arduino, esp32, coding, programimg, robot, arm robot,
    •         iot, google sheet, wifi, bluetooth, plc, scada, modbus
  2. - ดูคำอธิบายและตัวอย่างตามลิงค์ใน Youtube
  3. - เดี่๋ยวมีเวลามาเขียน ขยายความ  :)

 

 

  

 

  1. #include<Servo.h>  // add the servo libraries
  2. Servo myservo1;  // create servo object to control a servo
  3. Servo myservo2;
  4. Servo myservo3;
  5. Servo myservo4;
  6. Servo myservo5;
  7. Servo myservo6;
  8. int pos1 =90, pos2=90, pos3=90, pos4=90,  pos5=90, pos6=90; // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
  9. int PosIndex =1;
  10. int Joint1[7] = {80,120,120,120,160,160,120};
  11. int Joint2[7] = {110,120,92,92,120,120,120};
  12. int Joint3[7] = {100,100,100,100,100,100,100};
  13. int Joint4[7] = {80,80,80,80,80,80,80};
  14. int Joint5[7] = {60,60,60,60,60,60,60};
  15. int Joint6[7] = {60,60,60,115,115,60,60};
  16. //int armJoint[] = {90,90,90,90,90};
  17. const int right_X = A2; // define the right X pin to A2
  18. const int right_Y = A5; // define the right Y pin to A5
  19. const int right_key = 2; // define the right key pin to 7（that is the value of Z）
  20. const int left_X = A3; // define the left X pin to A3
  21. const int left_Y = A4; // define the left X pin to A4
  22. const int left_key = 11; //define the left key pin to 8（that is the value of Z）
  23. int x1,y1; // define the variable, used to save the joystick value it read.
  24. int x2,y2;
  25. bool z1,z2;
  26. int pos[7];
  27. char val;
  28. int incomingByte = 0;          // Received data byte
  29. String inputString = "";         // Used to store received content
  30. boolean newLineReceived = false; // Previous data end flag
  31. boolean startBit  = false;  //Acceptance Agreement Start Sign
  32. int num_reveice=0;
  33. void setup()
  34. {
  35.    // boot posture
  36.   pinMode(right_key, INPUT); // set the right/left key to INPUT
  37.   pinMode(left_key, INPUT);
  38.   myservo1.write(Joint1[0]);  
  39.   delay(1000);
  40.   myservo2.write(Joint2[0]);
  41.   myservo3.write(Joint3[0]);
  42.   myservo4.write(Joint4[0]);
  43.   myservo5.write(Joint5[0]);
  44.   myservo6.write(Joint6[0]);
  45.   delay(1000);
  46.  
  47.   delay(1500);
  48.   Serial.begin(9600); //  set the baud rate to 9600
  49. }
  50. void loop()
  51. {
  52.   myservo1.attach(3);  // set the control pin of servo 1 to D3
  53.   myservo2.attach(5);  // set the control pin of servo 2 to D5
  54.   myservo3.attach(6);   // set the control pin of servo 3 to D6
  55.   myservo4.attach(7);   // set the control pin of servo 4 to D9
  56.   myservo5.attach(8);
  57.   myservo6.attach(9);
  58. //  myservo5.attach(10);
  59. x2 = analogRead(right_X); //read the right X value
  60. y2 = analogRead(right_Y); // read the right Y value
  61. z2 = digitalRead(right_key); //// read the right Z value
  62. x1 = analogRead(left_X); //read the left X value
  63. y1 = analogRead(left_Y); //read the left Y value
  64. z1 = digitalRead(left_key); // read the left Z value
  65. while (Serial.available())
  66.   {
  67.     incomingByte = Serial.read();              //One byte by byte, the next sentence is read into a string array to form a completed packet
  68.     Serial.println(incomingByte);
  69.     if (incomingByte == '%')
  70.     {
  71.       num_reveice = 0;
  72.       startBit = true;
  73.     }
  74.     if (startBit == true)
  75.     {
  76.       num_reveice++;
  77.       inputString += (char) incomingByte;    
  78.     }
  79.     if (startBit == true && incomingByte == '#')
  80.     {
  81.       newLineReceived = true;
  82.       startBit = false;
  83.     }
  84.    
  85.     if(num_reveice >= 20)
  86.     {
  87.       num_reveice = 0;
  88.       startBit = false;
  89.       newLineReceived = false;
  90.       inputString = "";
  91.     }  
  92.   }
  93. if(newLineReceived)
  94. {
  95.     switch(inputString[1])  
  96.     {
  97.       case 'B':  T_left();  break;   // turn left
  98.       case 'C':  T_right();  break;//turn right
  99.       case 'A':  RB();  break;// the lower arm will draw back
  100.       case 'D':  RF();  break;// the lower arm will  stretch out
  101.       case '5':  ZK();  break;//close the claw
  102.       case '6':  ZB();  break;//close the claw
  103.       case '4':  LB();  break;//the upper arm will lift up
  104.       case '7':  LF();  break;//the upper arm will go down
  105.       case '8':  WC();  break;
  106.       case '9':  WW();  break;
  107.       case 'E':  GO();  break;
  108.       case 'F':  GC();  break;
  109.       case 'K':  Run();  break;
  110.       case 'J':  Run5();  break;
  111.       case 'L':  Jog1();  break;
  112.       case 'M':  Jog2();  break;
  113.       case 'N':  Jog3();  break;
  114.       case 'O':  Jog4();  break;
  115.       case 'P':  Jog5();  break;
  116.       case 'Q':  Jog6();  break;
  117.       case 'R':  Jog7();  break;
  118.      // case 'M':  Set1();  break;
  119.       default:break;
  120.     }
  121.       inputString = "";   // clear the string
  122.       newLineReceived = false;
  123. }  
  124. Serial.println(z1);
  125. delay(200);
  126.    if(z1 == HIGH)
  127.     {
  128.      Run();
  129.     }
  130. }  
  131. // Jog1
  132. void Jog1()
  133. {
  134.   PosIndex = 0;
  135.   myservo1.write(Joint1[PosIndex]);  
  136.   //delay(1000);
  137.   myservo2.write(Joint2[PosIndex]);
  138.   myservo3.write(Joint3[PosIndex]);
  139.   myservo4.write(Joint4[PosIndex]);
  140.   myservo5.write(Joint5[PosIndex]);
  141.   myservo6.write(Joint6[PosIndex]);
  142.   delay(2000);
  143.  
  144. }
  145. // Jog2
  146. void Jog2()
  147. {
  148.   PosIndex = 1;
  149.   myservo1.write(Joint1[PosIndex]);  
  150.   //delay(1000);
  151.   myservo2.write(Joint2[PosIndex]);
  152.   myservo3.write(Joint3[PosIndex]);
  153.   myservo4.write(Joint4[PosIndex]);
  154.   myservo5.write(Joint5[PosIndex]);
  155.   myservo6.write(Joint6[PosIndex]);
  156.   delay(2000);
  157.  
  158. }
  159. // Jog3
  160. void Jog3()
  161. {
  162.   PosIndex = 2;
  163.   myservo1.write(Joint1[PosIndex]);  
  164.   //delay(1000);
  165.   myservo2.write(Joint2[PosIndex]);
  166.   myservo3.write(Joint3[PosIndex]);
  167.   myservo4.write(Joint4[PosIndex]);
  168.   myservo5.write(Joint5[PosIndex]);
  169.   myservo6.write(Joint6[PosIndex]);
  170.   delay(2000);
  171.  
  172. }
  173. // Jog4
  174. void Jog4()
  175. {
  176.   PosIndex = 3;
  177.   myservo1.write(Joint1[PosIndex]);  
  178.   //delay(1000);
  179.   myservo2.write(Joint2[PosIndex]);
  180.   myservo3.write(Joint3[PosIndex]);
  181.   myservo4.write(Joint4[PosIndex]);
  182.   myservo5.write(Joint5[PosIndex]);
  183.   myservo6.write(Joint6[PosIndex]);
  184.   delay(2000);
  185.  
  186. }
  187. // Jog5
  188. void Jog5()
  189. {
  190.   PosIndex = 4;
  191.   myservo1.write(Joint1[PosIndex]);  
  192.   //delay(1000);
  193.   myservo2.write(Joint2[PosIndex]);
  194.   myservo3.write(Joint3[PosIndex]);
  195.   myservo4.write(Joint4[PosIndex]);
  196.   myservo5.write(Joint5[PosIndex]);
  197.   myservo6.write(Joint6[PosIndex]);
  198.   delay(2000);
  199.  
  200. }
  201. // Jog6
  202. void Jog6()
  203. {
  204.   PosIndex = 5;
  205.   myservo1.write(Joint1[PosIndex]);  
  206.   //delay(1000);
  207.   myservo2.write(Joint2[PosIndex]);
  208.   myservo3.write(Joint3[PosIndex]);
  209.   myservo4.write(Joint4[PosIndex]);
  210.   myservo5.write(Joint5[PosIndex]);
  211.   myservo6.write(Joint6[PosIndex]);
  212.   delay(2000);
  213.  
  214. }
  215. // Jog7
  216. void Jog7()
  217. {
  218.   PosIndex = 6;
  219.   myservo1.write(Joint1[PosIndex]);  
  220.   //delay(1000);
  221.   myservo2.write(Joint2[PosIndex]);
  222.   myservo3.write(Joint3[PosIndex]);
  223.   myservo4.write(Joint4[PosIndex]);
  224.   myservo5.write(Joint5[PosIndex]);
  225.   myservo6.write(Joint6[PosIndex]);
  226.   delay(2000);
  227.  
  228. }
  229. // Run 1
  230. void Run()
  231. {
  232. for (byte i = 0; i < 7; i = i + 1) {
  233.   myservo1.write(Joint1[i]);  
  234.   //delay(1000);
  235.   myservo2.write(Joint2[i]);
  236.   myservo3.write(Joint3[i]);
  237.   myservo4.write(Joint4[i]);
  238.   myservo5.write(Joint5[i]);
  239.   myservo6.write(Joint6[i]);
  240.   delay(2000);
  241.  
  242. }
  243. }
  244. // Run 5 cycle
  245. void Run5()
  246. {
  247. for (byte j = 0; j < 5; j = j + 1) {
  248. for (byte i = 0; i < 6; i = i + 1) {
  249.   myservo1.write(Joint1[i]);  
  250.   //delay(1000);
  251.   myservo2.write(Joint2[i]);
  252.   myservo3.write(Joint3[i]);
  253.   myservo4.write(Joint4[i]);
  254.   myservo5.write(Joint5[i]);
  255.   myservo6.write(Joint6[i]);
  256.   delay(2000);
  257.  
  258. }
  259. }
  260. }  
  261.  
  262.  
  263. //}
  264. //**************************************************
  265. // turn left
  266. void T_left()
  267. {
  268.     (Joint1[PosIndex])=(Joint1[PosIndex])+3;
  269.     myservo1.write((Joint1[PosIndex]));
  270.     delay(100);
  271.     if((Joint1[PosIndex])>150)
  272.     {
  273.       (Joint1[PosIndex])=150;
  274.     }
  275. }
  276. //turn right
  277. void T_right()
  278. {
  279.     (Joint1[PosIndex])=(Joint1[PosIndex])-3;
  280.     myservo1.write((Joint1[PosIndex]));
  281.     delay(100);
  282.     if((Joint1[PosIndex])<50)
  283.     {
  284.       (Joint1[PosIndex])=50;
  285.     }
  286. }
  287. //********************************************
  288. //close the claw
  289. void ZK()
  290. {
  291.       (Joint4[PosIndex])=(Joint4[PosIndex])-3;
  292.       //Serial.println((Joint4[PosIndex]));
  293.       myservo4.write((Joint4[PosIndex]));
  294.       delay(100);
  295.       if((Joint4[PosIndex])<30)
  296.       {
  297.         (Joint4[PosIndex])=30;
  298.       }
  299. }
  300. // open the claw
  301. void ZB()
  302. {
  303.     (Joint4[PosIndex])=(Joint4[PosIndex])+3;
  304.       //Serial.println((Joint4[PosIndex]));
  305.       myservo4.write((Joint4[PosIndex]));
  306.       delay(100);
  307.       if((Joint4[PosIndex])>150)
  308.       {
  309.         (Joint4[PosIndex])=150;
  310.       }
  311. }
  312. //******************************************
  313. // the lower arm will  stretch out
  314. void RF()
  315. {
  316.     (Joint2[PosIndex])=(Joint2[PosIndex])-3;
  317.     myservo2.write((Joint2[PosIndex]));
  318.     delay(100);
  319.     if((Joint2[PosIndex])<30)
  320.     {
  321.       (Joint2[PosIndex])=30;
  322.     }
  323. }
  324. // the lower arm will draw back
  325. void RB()
  326. {
  327.     (Joint2[PosIndex])=(Joint2[PosIndex])+3;
  328.     myservo2.write((Joint2[PosIndex]));
  329.     delay(100);
  330.     if((Joint2[PosIndex])>150)
  331.     {
  332.       (Joint2[PosIndex])=150;
  333.     }
  334. }
  335. //***************************************
  336. //the upper arm will lift up  
  337. void LB()
  338. {
  339.   (Joint3[PosIndex])=(Joint3[PosIndex])+3;
  340.     myservo3.write((Joint3[PosIndex]));
  341.     delay(100);
  342.     if((Joint3[PosIndex])>150)
  343.     {
  344.       (Joint3[PosIndex])=150;
  345.     }
  346. }
  347. //the upper arm will go down  
  348. void LF()
  349. {
  350.   (Joint3[PosIndex])=(Joint3[PosIndex])-3;
  351.     myservo3.write((Joint3[PosIndex]));
  352.     delay(100);
  353.     if((Joint3[PosIndex])<30)
  354.     {
  355.       (Joint3[PosIndex])=30;
  356.     }
  357. }
  358. //***************************************
  359. //the wrist cw  
  360. void WC()
  361. {
  362.   (Joint5[PosIndex])=(Joint5[PosIndex])+3;
  363.     myservo5.write((Joint5[PosIndex]));
  364.     delay(100);
  365.     if((Joint5[PosIndex])>140)
  366.     {
  367.       (Joint5[PosIndex])=140;
  368.     }
  369. }
  370. //the wrist ccw  
  371. void WW()
  372. {
  373.   (Joint5[PosIndex])=(Joint5[PosIndex])-3;
  374.     myservo5.write((Joint5[PosIndex]));
  375.     delay(100);
  376.     if((Joint5[PosIndex])<30)
  377.     {
  378.       (Joint5[PosIndex])=30;
  379.     }
  380. }
  381. //***************************************
  382. //the Grip open  
  383. void GO()
  384. {
  385.   (Joint6[PosIndex])=(Joint6[PosIndex])+3;
  386.     myservo6.write((Joint6[PosIndex]));
  387.     delay(100);
  388.     if((Joint6[PosIndex])>140)
  389.     {
  390.       (Joint6[PosIndex])=140;
  391.     }
  392. }
  393. //the Grip close  
  394. void GC()
  395. {
  396.   (Joint6[PosIndex])=(Joint6[PosIndex])-3;
  397.     myservo6.write((Joint6[PosIndex]));
  398.     delay(100);
  399.     if((Joint6[PosIndex])<30)
  400.     {
  401.       (Joint6[PosIndex])=30;
  402.     }
  403. }
Visitors: 18,170