Advertisement
safwan092

Untitled

May 15th, 2022
45
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.44 KB | None | 0 0
  1. #include <SoftwareSerial.h>
  2. #include <Servo.h>
  3.  
  4.  
  5. SoftwareSerial softSerial(11, 8); //TX_BT --> D11 / RX_BT --> D8
  6.  
  7. int rx = 0;
  8. int temp = 0;
  9.  
  10. Servo servo1;
  11. Servo servo2;
  12. Servo servo3;
  13. Servo servo4;
  14. Servo servo5;
  15.  
  16. int pos1 = 120;
  17. int pos2 = 60;
  18. int pos3 = 60;
  19. int pos4 = 60;
  20. int pos5 = 60;
  21.  
  22. //1: 30-120 = 90
  23. //2: 60-150 = 90
  24. //3: 60-150 = 90
  25. //4: 60-150 = 90
  26. //5: 60-150 = 90
  27.  
  28. void setup() {
  29. Serial.begin(9600);
  30. softSerial.begin(9600);
  31. servoA();
  32. servo1.write(pos1);
  33. servo2.write(pos2);
  34. servo3.write(pos3);
  35. servo4.write(pos4);
  36. servo5.write(pos5);
  37. delay(1000);
  38. }
  39.  
  40. void loop() {
  41.  
  42. if (softSerial.available() > 0) {
  43. rx = softSerial.read();
  44. softSerial.flush();
  45. //Serial.println(rx);
  46. }
  47.  
  48. if (rx == 1 && temp != 1) {
  49. softSerial.flush();
  50. temp = 1;
  51. servoA();
  52. all_fingers_open();
  53. delay(1000);
  54. servoD();
  55. Serial.println("1");
  56. }
  57. else if (rx == 2 && temp != 2) {
  58. softSerial.flush();
  59. temp = 2;
  60. servoA();
  61. all_fingers_close();
  62. delay(1000);
  63. servoD();
  64. Serial.println("2");
  65. }
  66. else if (rx == 3 && temp != 3) {
  67. softSerial.flush();
  68. temp = 3;
  69. servoA();
  70. finger1open();
  71. four_fingers_close();
  72. delay(1000);
  73. servoD();
  74. Serial.println("3");
  75. }
  76. else if (rx == 4 && temp != 4) {
  77. softSerial.flush();
  78. temp = 4;
  79. servoA();
  80. finger1close();
  81. four_fingers_open();
  82. delay(1000);
  83. servoD();
  84. Serial.println("4");
  85. }
  86. delay(100);
  87.  
  88. }// end of LOOP
  89.  
  90.  
  91. void all_fingers_close() {
  92. servo1.write(30);
  93. servo2.write(150);
  94. servo3.write(150);
  95. servo4.write(150);
  96. servo5.write(150);
  97. delay(15);
  98. }
  99. void all_fingers_open() {
  100. servo1.write(120);
  101. servo2.write(60);
  102. servo3.write(60);
  103. servo4.write(60);
  104. servo5.write(60);
  105. delay(15);
  106. }
  107.  
  108. void finger1close() {
  109. servo1.write(30);
  110. delay(15);
  111. }
  112. void finger1open() {
  113. servo1.write(120);
  114. delay(15);
  115. }
  116.  
  117. void four_fingers_close() {
  118. servo2.write(150);
  119. servo3.write(150);
  120. servo4.write(150);
  121. servo5.write(150);
  122. delay(15);
  123. }
  124.  
  125. void four_fingers_open() {
  126. servo2.write(60);
  127. servo3.write(60);
  128. servo4.write(60);
  129. servo5.write(60);
  130. delay(15);
  131. }
  132.  
  133. void servoD() {
  134. servo1.detach();
  135. servo2.detach();
  136. servo3.detach();
  137. servo4.detach();
  138. servo5.detach();
  139. }
  140.  
  141. void servoA() {
  142. servo1.attach(3);
  143. servo2.attach(5);
  144. servo3.attach(6);
  145. servo4.attach(9);
  146. servo5.attach(10);
  147. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement