Tu pa je zapisana celotna programska koda robotske roke Arduino: 1. /* 2. DIY Arduino Robot Arm Smartphone Control 3. by Dejan, www.HowToMechatronics.com 4. */ 5. 6. #include 7. #include 8. 9. Servo servo01; 10. Servo servo02; 11. Servo servo03; 12. Servo servo04; 13. Servo servo05; 14. Servo servo06; 15. 16. SoftwareSerial Bluetooth(3, 4); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX) 17. 18. int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position 19. int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position 20. int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps 21. int speedDelay = 20; 22. int index = 0; 23. String dataIn = ""; 24. 25. voidsetup(){ 26. servo01.attach(5); 27. servo02.attach(6); 28. servo03.attach(7); 29. servo04.attach(8); 30. servo05.attach(9); 31. servo06.attach(10); 32. Bluetooth.begin(38400); // Default baud rate of the Bluetooth module 33. Bluetooth.setTimeout(1); 34. delay(20); 35. // Robot arm initial position 36. servo1PPos = 90; 37. servo01.write(servo1PPos); 38. servo2PPos = 150; 39. servo02.write(servo2PPos); 40. servo3PPos = 35; 41. servo03.write(servo3PPos); 42. servo4PPos = 140; 43. servo04.write(servo4PPos); 44. servo5PPos = 85; 45. servo05.write(servo5PPos); 46. servo6PPos = 80; 47. servo06.write(servo6PPos); 48. } 49. 50. voidloop(){ 51. // Check for incoming data 52. if(Bluetooth.available()>0){ 53. dataIn = Bluetooth.readString(); // Read the data as string 54. 55. // If "Waist" slider has changed value - Move Servo 1 to position 56. if(dataIn.startsWith("s1")){ 57. String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s1120" to "120" 58. servo1Pos = dataInS.toInt(); // Convert the string into integer 59. // We use for loops so we can control the speed of the servo 60. // If previous position is bigger then current position 61. if(servo1PPos > servo1Pos){ 62. for(int j = servo1PPos; j >= servo1Pos; j--){// Run servo down 63. servo01.write(j); 64. delay(20); // defines the speed at which the servo rotates 65. } 66. } 67. // If previous position is smaller then current position 68. if(servo1PPos < servo1Pos){ 69. for(int j = servo1PPos; j <= servo1Pos; j++){// Run servo up 70. servo01.write(j); 71. delay(20); 72. } 73. } 74. servo1PPos = servo1Pos; // set current position as previous position 75. } 76. 77. // Move Servo 2 78. if(dataIn.startsWith("s2")){ 79. String dataInS = dataIn.substring(2, dataIn.length()); 80. servo2Pos = dataInS.toInt(); 81. 82. if(servo2PPos > servo2Pos){ 83. for(int j = servo2PPos; j >= servo2Pos; j--){ 84. servo02.write(j); 85. delay(50); 86. } 87. } 88. if(servo2PPos < servo2Pos){ 89. for(int j = servo2PPos; j <= servo2Pos; j++){ 90. servo02.write(j); 91. delay(50); 92. } 93. } 94. servo2PPos = servo2Pos; 95. } 96. // Move Servo 3 97. if(dataIn.startsWith("s3")){ 98. String dataInS = dataIn.substring(2, dataIn.length()); 99. servo3Pos = dataInS.toInt(); 100. if(servo3PPos > servo3Pos){ 101. for(int j = servo3PPos; j >= servo3Pos; j--){ 102. servo03.write(j); 103. delay(30); 104. } 105. } 106. if(servo3PPos < servo3Pos){ 107. for(int j = servo3PPos; j <= servo3Pos; j++){ 108. servo03.write(j); 109. delay(30); 110. } 111. } 112. servo3PPos = servo3Pos; 113. } 114. // Move Servo 4 115. if(dataIn.startsWith("s4")){ 116. String dataInS = dataIn.substring(2, dataIn.length()); 117. servo4Pos = dataInS.toInt(); 118. if(servo4PPos > servo4Pos){ 119. for(int j = servo4PPos; j >= servo4Pos; j--){ 120. servo04.write(j); 121. delay(30); 122. } 123. } 124. if(servo4PPos < servo4Pos){ 125. for(int j = servo4PPos; j <= servo4Pos; j++){ 126. servo04.write(j); 127. delay(30); 128. } 129. } 130. servo4PPos = servo4Pos; 131. } 132. // Move Servo 5 133. if(dataIn.startsWith("s5")){ 134. String dataInS = dataIn.substring(2, dataIn.length()); 135. servo5Pos = dataInS.toInt(); 136. if(servo5PPos > servo5Pos){ 137. for(int j = servo5PPos; j >= servo5Pos; j--){ 138. servo05.write(j); 139. delay(30); 140. } 141. } 142. if(servo5PPos < servo5Pos){ 143. for(int j = servo5PPos; j <= servo5Pos; j++){ 144. servo05.write(j); 145. delay(30); 146. } 147. } 148. servo5PPos = servo5Pos; 149. } 150. // Move Servo 6 151. if(dataIn.startsWith("s6")){ 152. String dataInS = dataIn.substring(2, dataIn.length()); 153. servo6Pos = dataInS.toInt(); 154. if(servo6PPos > servo6Pos){ 155. for(int j = servo6PPos; j >= servo6Pos; j--){ 156. servo06.write(j); 157. delay(30); 158. } 159. } 160. if(servo6PPos < servo6Pos){ 161. for(int j = servo6PPos; j <= servo6Pos; j++){ 162. servo06.write(j); 163. delay(30); 164. } 165. } 166. servo6PPos = servo6Pos; 167. } 168. // If button "SAVE" is pressed 169. if(dataIn.startsWith("SAVE")){ 170. servo01SP[index] = servo1PPos; // save position into the array 171. servo02SP[index] = servo2PPos; 172. servo03SP[index] = servo3PPos; 173. servo04SP[index] = servo4PPos; 174. servo05SP[index] = servo5PPos; 175. servo06SP[index] = servo6PPos; 176. index++; // Increase the array index 177. } 178. // If button "RUN" is pressed 179. if(dataIn.startsWith("RUN")){ 180. runservo(); // Automatic mode - run the saved steps 181. } 182. // If button "RESET" is pressed 183. if( dataIn == "RESET"){ 184. memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0 185. memset(servo02SP, 0, sizeof(servo02SP)); 186. memset(servo03SP, 0, sizeof(servo03SP)); 187. memset(servo04SP, 0, sizeof(servo04SP)); 188. memset(servo05SP, 0, sizeof(servo05SP)); 189. memset(servo06SP, 0, sizeof(servo06SP)); 190. index = 0; // Index to 0 191. } 192. } 193. } 194. 195. // Automatic mode custom function - run the saved steps 196. voidrunservo(){ 197. while(dataIn != "RESET"){// Run the steps over and over again until "RESET" button is pressed 198. for(int i = 0; i <= index - 2; i++){// Run through all steps(index) 199. if(Bluetooth.available()>0){// Check for incomding data 200. dataIn = Bluetooth.readString(); 201. if( dataIn == "PAUSE"){// If button "PAUSE" is pressed 202. while(dataIn != "RUN"){// Wait until "RUN" is pressed again 203. if(Bluetooth.available()>0){ 204. dataIn = Bluetooth.readString(); 205. if( dataIn == "RESET"){ 206. break; 207. } 208. } 209. } 210. } 211. // If speed slider is changed 212. if(dataIn.startsWith("ss")){ 213. String dataInS = dataIn.substring(2, dataIn.length()); 214. speedDelay = dataInS.toInt(); // Change servo speed (delay time) 215. } 216. } 217. // Servo 1 218. if(servo01SP[i] == servo01SP[i + 1]){ 219. } 220. if(servo01SP[i]> servo01SP[i + 1]){ 221. for(int j = servo01SP[i]; j >= servo01SP[i + 1]; j--){ 222. servo01.write(j); 223. delay(speedDelay); 224. } 225. } 226. if(servo01SP[i]< servo01SP[i + 1]){ 227. for(int j = servo01SP[i]; j <= servo01SP[i + 1]; j++){ 228. servo01.write(j); 229. delay(speedDelay); 230. } 231. } 232. 233. // Servo 2 234. if(servo02SP[i] == servo02SP[i + 1]){ 235. } 236. if(servo02SP[i]> servo02SP[i + 1]){ 237. for(int j = servo02SP[i]; j >= servo02SP[i + 1]; j--){ 238. servo02.write(j); 239. delay(speedDelay); 240. } 241. } 242. if(servo02SP[i]< servo02SP[i + 1]){ 243. for(int j = servo02SP[i]; j <= servo02SP[i + 1]; j++){ 244. servo02.write(j); 245. delay(speedDelay); 246. } 247. } 248. 249. // Servo 3 250. if(servo03SP[i] == servo03SP[i + 1]){ 251. } 252. if(servo03SP[i]> servo03SP[i + 1]){ 253. for(int j = servo03SP[i]; j >= servo03SP[i + 1]; j--){ 254. servo03.write(j); 255. delay(speedDelay); 256. } 257. } 258. if(servo03SP[i]< servo03SP[i + 1]){ 259. for(int j = servo03SP[i]; j <= servo03SP[i + 1]; j++){ 260. servo03.write(j); 261. delay(speedDelay); 262. } 263. } 264. 265. // Servo 4 266. if(servo04SP[i] == servo04SP[i + 1]){ 267. } 268. if(servo04SP[i]> servo04SP[i + 1]){ 269. for(int j = servo04SP[i]; j >= servo04SP[i + 1]; j--){ 270. servo04.write(j); 271. delay(speedDelay); 272. } 273. } 274. if(servo04SP[i]< servo04SP[i + 1]){ 275. for(int j = servo04SP[i]; j <= servo04SP[i + 1]; j++){ 276. servo04.write(j); 277. delay(speedDelay); 278. } 279. } 280. 281. // Servo 5 282. if(servo05SP[i] == servo05SP[i + 1]){ 283. } 284. if(servo05SP[i]> servo05SP[i + 1]){ 285. for(int j = servo05SP[i]; j >= servo05SP[i + 1]; j--){ 286. servo05.write(j); 287. delay(speedDelay); 288. } 289. } 290. if(servo05SP[i]< servo05SP[i + 1]){ 291. for(int j = servo05SP[i]; j <= servo05SP[i + 1]; j++){ 292. servo05.write(j); 293. delay(speedDelay); 294. } 295. } 296. 297. // Servo 6 298. if(servo06SP[i] == servo06SP[i + 1]){ 299. } 300. if(servo06SP[i]> servo06SP[i + 1]){ 301. for(int j = servo06SP[i]; j >= servo06SP[i + 1]; j--){ 302. servo06.write(j); 303. delay(speedDelay); 304. } 305. } 306. if(servo06SP[i]< servo06SP[i + 1]){ 307. for(int j = servo06SP[i]; j <= servo06SP[i + 1]; j++){ 308. servo06.write(j); 309. delay(speedDelay); 310. } 311. } 312. } 313. } 314. } Upam, da vam je všeč tudi videoposnetek, ki prikazuje delovanje