API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 15th, 2017
93
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 23.12 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. INEM Robot Motoric 2017 - platform 1
  3. Copyright by Antonius (Ringlayer)
  4. www.ringlayer.net - www.ringlayer.com
  5. Prototype designed for Ringlayer Robotic
  6. Start Project on :
  7. ---------------------------------
  8. ringlayer-X453SA test # date
  9. Mon Jan 23 04:58:25 WIB 2017
  10. ---------------------------------
  11. known commands:
  12. mobility functions :
  13.  
  14. - f|speed => move forward with intended speed
  15. - b|speed => move backward with intended speed
  16. -ml|speed => left manouver with intended speed
  17. - mr|speed => right manouver with intended speed
  18. - r|speed => move right with intended speed
  19. - l|speed => move left with intended speed
  20. - s -> stop movement
  21.  
  22. sensor functions :
  23. - u|1 => front sonar
  24. - u|2 => right sonar
  25. - u|3 => left sonar
  26.  
  27. head movement functions :
  28. - h1|degree
  29. - h2|degree
  30. - h1_quick|degree
  31. - h2_quick|degree
  32. - h_init
  33. - h_reattach
  34. - h_redetach
  35. - h_all_rev|degree1|degree2
  36. - h_all|degree1|degree2
  37.  
  38.  
  39. arm movement functions :
  40. - a1|degree
  41. - a2|degree
  42. - a3|degree
  43. - a4|degree
  44. - a5|degree
  45. - a6|degree
  46. - a1_quick|degree
  47. - a2_quick|degree
  48. - a3_quick|degree
  49. - a4_quick|degree
  50. - a5_quick|degree
  51. - a_init1
  52. - a_init2
  53. - a_init3
  54. - a_reattach
  55. - a_redetach
  56. - a_all|degree1|degree2|degree3|degree4|degree5|degree6
  57. - a_all_rev|degree1|degree2|degree3|degree4|degree5|degree6
  58. */
  59.  
  60. #include <Servo.h>
  61. #include <NewPing.h>
  62. #include <string.h>
  63. #include <stdlib.h>
  64.  
  65. /* general servo vars */
  66. int _servo_delay_low_ms = 400;
  67. int _servo_delay_ms = 700;
  68. /* eof general servo vars */
  69.  
  70. /* vars for servo - head servo */
  71. int _servo_head_last_degree[2] = {100,30};
  72. int _servo_head_init_degree[2] = {100, 30};
  73.  
  74. Servo servo_head1;
  75. Servo servo_head2;
  76. int _servo_head1_max_left_degree = 10;
  77. int _servo_head1_max_right_degree = 170;
  78. int _servo_head1_pin = 22;
  79.  
  80. int _servo_head2_max_up_degree = 140;
  81. int _servo_head2_max_down_degree = 15;
  82. int _servo_head2_pin = 23;
  83. /* eof vars for servo - head servo */
  84.  
  85. /* vars for servo - arm servo */
  86. int _servo_arm_last_degree[6] = {90,90,90,90,90,90};
  87. int _servo_arm_init_degree_1[6] = {90,90,90,90,90,90};
  88. int _servo_arm_init_degree_2[6] = {90,90,90,90,90,90};
  89. int _servo_arm_init_degree_3[6] = {90,90,90,90,90,90};
  90.  
  91. Servo servo_arm1, servo_arm2, servo_arm3, servo_arm4, servo_arm5, servo_arm6;
  92. int _servo_arm1_max_left_degree = 40;
  93. int _servo_arm1_max_right_degree = 140;
  94. int _servo_arm1_pin = 24;
  95.  
  96. int _servo_arm2_max_left_degree = 40;
  97. int _servo_arm2_max_right_degree = 140;
  98. int _servo_arm2_pin = 25;
  99.  
  100. int _servo_arm3_max_left_degree = 40;
  101. int _servo_arm3_max_right_degree = 140;
  102. int _servo_arm3_pin = 26;
  103.  
  104. int _servo_arm4_max_left_degree = 40;
  105. int _servo_arm4_max_right_degree = 140;
  106. int _servo_arm4_pin = 27;
  107.  
  108. int _servo_arm5_max_left_degree = 40;
  109. int _servo_arm5_max_right_degree = 140;
  110. int _servo_arm5_pin = 28;
  111.  
  112. int _servo_arm6_max_left_degree = 40;
  113. int _servo_arm6_max_right_degree = 140;
  114. int _servo_arm6_pin = 29;
  115. /* eof vars for servo - arm servo */
  116.  
  117. /* start motoric vars */
  118. int speed_pin_right = 9;
  119. int right_pin1 = 5;
  120. int right_pin2 = 6;
  121. int speed_pin_left = 8;
  122. int left_pin1 = 3;
  123. int left_pin2 = 4;
  124. /* eof motoric vars */
  125.  
  126. /* start sonar vars */
  127. #define front_echo 14
  128. #define front_trigger 15
  129. #define right_echo 16
  130. #define right_trigger 17
  131. #define left_echo 18
  132. #define left_trigger 19
  133. #define MAX_DISTANCE 200
  134. int _distance = 0;
  135. int _tmp_check = 0;
  136. String send_sonar_data = "";
  137. String preceed_sonar_data = "";
  138. String _dist = "";
  139. NewPing front_sonar(front_trigger, front_echo, MAX_DISTANCE);
  140. NewPing left_sonar(left_trigger, left_echo, MAX_DISTANCE);
  141. NewPing right_sonar(right_trigger, right_echo, MAX_DISTANCE);
  142. /* eof sonar vars */
  143.  
  144.  
  145. void setup() {
  146. Serial.begin(115200);
  147.  
  148. pinMode(left_pin1,OUTPUT);
  149. pinMode(left_pin2,OUTPUT);
  150. pinMode(speed_pin_left,OUTPUT);
  151.  
  152. pinMode(right_pin1,OUTPUT);
  153. pinMode(right_pin2,OUTPUT);
  154. pinMode(speed_pin_right,OUTPUT);
  155.  
  156. digitalWrite(speed_pin_right, HIGH);
  157. digitalWrite(speed_pin_left, HIGH);
  158.  
  159. servo_arm1.attach(_servo_arm1_pin);
  160. servo_arm2.attach(_servo_arm2_pin);
  161. servo_arm3.attach(_servo_arm3_pin);
  162. servo_arm4.attach(_servo_arm4_pin);
  163. servo_arm5.attach(_servo_arm5_pin);
  164. servo_arm6.attach(_servo_arm6_pin);
  165.  
  166. servo_head1.attach(_servo_head1_pin);
  167. servo_head2.attach(_servo_head2_pin);
  168.  
  169. }
  170.  
  171. /* ------------------------------------------------------ */
  172. /* system functions */
  173.  
  174. /*
  175. delay with serial check, created by Ringlayer on
  176. Wed Feb 22 23:40:49 WIB 2017
  177. */
  178.  
  179. void _delay(int second) {
  180. int i;
  181. int mili_second = second * 1000;
  182. int count_unblock = mili_second / 100;
  183. delay(100);
  184. for (i = 0; i < count_unblock; i++) {
  185. _main(1);
  186. delay(100);
  187. }
  188. }
  189.  
  190. void _delay_ms(int mili_second) {
  191. int i;
  192. int count_unblock = mili_second / 100;
  193. delay(100);
  194. for (i = 0; i < count_unblock; i++) {
  195. _main(1);
  196. delay(100);
  197. }
  198. }
  199.  
  200. /* eof system functions */
  201.  
  202. /* ------------------------------------------------------ */
  203. /* sonar function */
  204.  
  205. void _get_distances_from_sonars(int _num) {
  206. switch (_num) {
  207. case 1:
  208. _tmp_check = front_sonar.ping_cm();
  209. preceed_sonar_data = "f|";
  210. break;
  211. case 2:
  212. _tmp_check = right_sonar.ping_cm();
  213. preceed_sonar_data = "r|";
  214. break;
  215. case 3:
  216. _tmp_check = left_sonar.ping_cm();
  217. preceed_sonar_data = "l|";
  218. break;
  219. }
  220. if ((_tmp_check > 1) && (_tmp_check < 201)) {
  221. _distance = _tmp_check;
  222. _dist = String(_distance);
  223. _dist.trim();
  224. if (_dist.length() > 0) {
  225. send_sonar_data = preceed_sonar_data + _dist;
  226. Serial.println(send_sonar_data);
  227. }
  228. }
  229. delay(50);
  230. }
  231. /* eof sonar function */
  232.  
  233.  
  234. /* ------------------------------------------------------ */
  235. /* motor functions */
  236.  
  237. void _diam() {
  238. _op_motor_kanan(0, "stop");
  239. _op_motor_kiri(0, "stop");
  240. }
  241.  
  242. void mundur(int speed) {
  243. if (speed == 0) {
  244. speed = 255;
  245. }
  246. _op_motor_kanan(speed, "mundur");
  247. _op_motor_kiri(speed, "mundur");
  248. }
  249.  
  250. void maju(int speed) {
  251. if (speed == 0) {
  252. speed = 255;
  253. }
  254. _op_motor_kanan(speed, "maju");
  255. _op_motor_kiri(speed, "maju");
  256. }
  257.  
  258. void belok_kanan(int speed) {
  259. if (speed == 0) {
  260. speed = 255;
  261. }
  262. _op_motor_kanan(speed, "mundur");
  263. _op_motor_kiri(speed, "maju");
  264. if (speed > 220) {
  265. _delay_ms(750);
  266. }
  267. else if((speed < 221) and (speed > 170)) {
  268. _delay_ms(800);
  269. }
  270. else {
  271. _delay_ms(900);
  272. }
  273. _diam();
  274. }
  275.  
  276. void belok_kiri(int speed) {
  277. if (speed == 0) {
  278. speed = 255;
  279. }
  280. _op_motor_kanan(speed, "maju");
  281. _op_motor_kiri(speed, "mundur");
  282. if (speed > 220) {
  283. _delay_ms(750);
  284. }
  285. else if((speed < 221) and (speed > 170)) {
  286. _delay_ms(800);
  287. }
  288. else {
  289. _delay_ms(900);
  290. }
  291. _diam();
  292. }
  293.  
  294. void manuver_kanan(int speed) {
  295. if (speed == 0) {
  296. speed = 255;
  297. }
  298. _op_motor_kanan(speed, "mundur");
  299. _op_motor_kiri(speed, "maju");
  300. }
  301.  
  302. void manuver_kiri(int speed) {
  303. if (speed == 0) {
  304. speed = 255;
  305. }
  306. _op_motor_kanan(speed, "maju");
  307. _op_motor_kiri(speed, "mundur");
  308. }
  309.  
  310. void _op_motor_kanan(int speed, const char *op) {
  311. analogWrite(speed_pin_right, speed);
  312. if (op == "mundur") {
  313. digitalWrite(right_pin1, LOW);
  314. digitalWrite(right_pin2, HIGH);
  315. }
  316. else if (op == "maju") {
  317. digitalWrite(right_pin1, HIGH);
  318. digitalWrite(right_pin2, LOW);
  319. }
  320. else if (op == "stop") {
  321. digitalWrite(right_pin1, LOW);
  322. digitalWrite(right_pin2, LOW);
  323. }
  324. }
  325.  
  326. void _op_motor_kiri(int speed, const char *op) {
  327. analogWrite(speed_pin_left, speed);
  328. if (op == "maju") {
  329. digitalWrite(left_pin1, LOW);
  330. digitalWrite(left_pin2, HIGH);
  331. }
  332. else if (op == "mundur") {
  333. digitalWrite(left_pin1, HIGH);
  334. digitalWrite(left_pin2, LOW);
  335. }
  336. else if (op == "stop") {
  337. digitalWrite(left_pin1, LOW);
  338. digitalWrite(left_pin2, LOW);
  339. }
  340. }
  341. /* eof motor functions */
  342.  
  343. /* ------------------------------------------------------ */
  344. /* common servo operations */
  345.  
  346. int _adjust_degree(int _degree) {
  347. if (_degree == 0) {
  348. _degree = 1;
  349. }
  350. if (_degree == 180) {
  351. _degree = 179;
  352. }
  353. return _degree;
  354. }
  355.  
  356. void _servo_init_position(int _init_degree,Servo _servo) {
  357. if (_init_degree == 0) {
  358. _init_degree = 1;
  359. }
  360. _servo.write(_init_degree);
  361. _delay_ms(_servo_delay_low_ms);
  362. }
  363.  
  364. void _servo_move(int _servo_ar_num, int _servo_degree, String mode, Servo _servo) {
  365. int _orig_servo_degree = 0;
  366.  
  367. if (mode == "head") {
  368. _orig_servo_degree = _servo_head_last_degree[_servo_ar_num];
  369. }
  370. else {
  371. _orig_servo_degree = _servo_arm_last_degree[_servo_ar_num];
  372. }
  373. if (_orig_servo_degree > _servo_degree) {
  374. while (_orig_servo_degree >= _servo_degree) {
  375. _servo.write(_orig_servo_degree);
  376. delay(14);
  377. _orig_servo_degree -= 1;
  378. }
  379. }
  380. else if (_orig_servo_degree < _servo_degree) {
  381. while (_orig_servo_degree <= _servo_degree) {
  382. _servo.write(_orig_servo_degree);
  383. delay(14);
  384. _orig_servo_degree += 1;
  385. }
  386. }
  387. if (mode == "head") {
  388. _servo_head_last_degree[_servo_ar_num] = _orig_servo_degree;
  389. }
  390. else if (mode == "arm") {
  391. _servo_arm_last_degree[_servo_ar_num] = _orig_servo_degree;
  392. }
  393. _delay_ms(_servo_delay_ms);
  394. }
  395.  
  396.  
  397. void _servo_quick_move(int _servo_ar_num, int _servo_degree, String mode, Servo _servo) {
  398. int _orig_servo_degree = _servo_degree;
  399.  
  400. _servo.write(_servo_degree);
  401. if (mode == "head") {
  402. _servo_head_last_degree[_servo_ar_num] = _orig_servo_degree;
  403. }
  404. else if (mode == "arm") {
  405. _servo_arm_last_degree[_servo_ar_num] = _orig_servo_degree;
  406. }
  407. _delay_ms(_servo_delay_ms);
  408. }
  409. /* eof common servo operations */
  410.  
  411. /* ------------------------------------------------------ */
  412. /* attach and deach servo operations */
  413.  
  414. void _reattach_head_servo() {
  415. servo_head1.attach(_servo_head1_pin);
  416. servo_head2.attach(_servo_head2_pin);
  417. }
  418.  
  419. void _redetach_head_servo() {
  420. if (servo_head1.attached()) {
  421. servo_head1.detach();
  422. }
  423. if (servo_head2.attached()) {
  424. servo_head2.detach();
  425. }
  426. }
  427.  
  428. void _reattach_arm_servo() {
  429. servo_arm1.attach(_servo_arm1_pin);
  430. servo_arm2.attach(_servo_arm2_pin);
  431. servo_arm3.attach(_servo_arm3_pin);
  432. servo_arm4.attach(_servo_arm4_pin);
  433. servo_arm5.attach(_servo_arm5_pin);
  434. servo_arm6.attach(_servo_arm6_pin);
  435. }
  436.  
  437. void _redetach_arm_servo() {
  438. if (servo_arm1.attached()) {
  439. servo_arm1.detach();
  440. }
  441. if (servo_arm2.attached()) {
  442. servo_arm2.detach();
  443. }
  444. if (servo_arm3.attached()) {
  445. servo_arm3.detach();
  446. }
  447. if (servo_arm4.attached()) {
  448. servo_arm4.detach();
  449. }
  450. if (servo_arm5.attached()) {
  451. servo_arm5.detach();
  452. }
  453. if (servo_arm6.attached()) {
  454. servo_arm6.detach();
  455. }
  456. }
  457. /* eof attach and deach servo operations */
  458.  
  459.  
  460. /* ------------------------------------------------------ */
  461. /* head servo operations */
  462.  
  463. void _head_joint_1_degree(int _degree, String mode) {
  464. /*
  465. Serial.println("head1 degree");
  466. Serial.println(_degree);
  467. */
  468. _degree = _adjust_degree(_degree);
  469. if (mode == "sweep") {
  470. _servo_move(0, _degree, "head", servo_head1);
  471. }
  472. else if (mode == "quick") {
  473. _servo_quick_move(0, _degree, "head", servo_head1);
  474. }
  475. }
  476.  
  477. void _head_joint_2_degree(int _degree, String mode) {
  478. _degree = _adjust_degree(_degree);
  479. if (mode == "sweep") {
  480. _servo_move(0, _degree, "head", servo_head2);
  481. }
  482. else if (mode == "quick") {
  483. _servo_quick_move(0, _degree, "head", servo_head2);
  484. }
  485. }
  486.  
  487. void _head_all_degree(int _degree1, int _degree2, String mode) {
  488. _degree1 = _adjust_degree(_degree1);
  489. _degree2 = _adjust_degree(_degree2);
  490. _head_joint_1_degree(_degree1, mode);
  491. _delay_ms(_servo_delay_low_ms);
  492. _head_joint_2_degree(_degree2, mode);
  493. _delay_ms(_servo_delay_low_ms);
  494. }
  495.  
  496. void _head_all_degree_rev(int _degree1, int _degree2, String mode) {
  497. _degree1 = _adjust_degree(_degree1);
  498. _degree2 = _adjust_degree(_degree2);
  499. _head_joint_2_degree(_degree2, mode);
  500. _delay_ms(_servo_delay_low_ms);
  501. _head_joint_1_degree(_degree1, mode);
  502. _delay_ms(_servo_delay_low_ms);
  503. }
  504.  
  505. void _head_init() {
  506.  
  507. Serial.println("head init");
  508.  
  509. _head_joint_1_init(_servo_head_init_degree[0]);
  510. _servo_head_last_degree[0] = _servo_head_init_degree[0];
  511. _delay_ms(_servo_delay_low_ms);
  512. _head_joint_2_init(_servo_head_init_degree[1]);
  513. _servo_head_last_degree[1] = _servo_head_init_degree[1];
  514. _delay_ms(_servo_delay_low_ms);
  515. }
  516.  
  517. void _head_joint_1_init(int _init_degree) {
  518. _servo_head_last_degree[0] = _init_degree;
  519. _servo_init_position(_init_degree, servo_head1);
  520.  
  521. _delay_ms(_servo_delay_ms);
  522. }
  523.  
  524. void _head_joint_2_init(int _init_degree) {
  525. _servo_head_last_degree[1] = _init_degree;
  526. _servo_init_position(_init_degree,servo_head2);
  527. _delay_ms(_servo_delay_ms);
  528. }
  529. /* eof head servo operations */
  530.  
  531. /* ------------------------------------------------------ */
  532. /* arm servo operations */
  533. void _arm1_init(int _init_degree) {
  534. _servo_arm_last_degree[0] = _init_degree;
  535. _servo_init_position(_init_degree, servo_arm1);
  536. _delay_ms(_servo_delay_ms);
  537. }
  538.  
  539. void _arm2_init(int _init_degree) {
  540. _servo_arm_last_degree[1] = _init_degree;
  541. _servo_init_position(_init_degree, servo_arm2);
  542. _delay_ms(_servo_delay_ms);
  543. }
  544.  
  545. void _arm3_init(int _init_degree) {
  546. _servo_arm_last_degree[2] = _init_degree;
  547. _servo_init_position(_init_degree, servo_arm3);
  548. _delay_ms(_servo_delay_ms);
  549. }
  550.  
  551. void _arm4_init(int _init_degree) {
  552. _servo_arm_last_degree[3] = _init_degree;
  553. _servo_init_position(_init_degree, servo_arm4);
  554. _delay_ms(_servo_delay_ms);
  555. }
  556.  
  557. void _arm5_init(int _init_degree) {
  558. _servo_arm_last_degree[4] = _init_degree;
  559. _servo_init_position(_init_degree, servo_arm5);
  560. _delay_ms(_servo_delay_ms);
  561. }
  562.  
  563. void _arm6_init(int _init_degree) {
  564. _servo_arm_last_degree[5] = _init_degree;
  565. _servo_init_position(_init_degree, servo_arm6);
  566. _delay_ms(_servo_delay_ms);
  567. }
  568.  
  569. void _arm_init_1() {
  570. _arm1_init(_servo_arm_init_degree_1[0]);
  571. _arm2_init(_servo_arm_init_degree_1[1]);
  572. _arm3_init(_servo_arm_init_degree_1[2]);
  573. _arm4_init(_servo_arm_init_degree_1[3]);
  574. _arm5_init(_servo_arm_init_degree_1[4]);
  575. _arm6_init(_servo_arm_init_degree_1[5]);
  576.  
  577. _servo_arm_last_degree[0] = _servo_arm_init_degree_1[0];
  578. _servo_arm_last_degree[1] = _servo_arm_init_degree_1[1];
  579. _servo_arm_last_degree[2] = _servo_arm_init_degree_1[2];
  580. _servo_arm_last_degree[3] = _servo_arm_init_degree_1[3];
  581. _servo_arm_last_degree[4] = _servo_arm_init_degree_1[4];
  582. _servo_arm_last_degree[5] = _servo_arm_init_degree_1[5];
  583. }
  584.  
  585. void _arm_init_2() {
  586. _arm1_init(_servo_arm_init_degree_2[0]);
  587. _arm2_init(_servo_arm_init_degree_2[1]);
  588. _arm3_init(_servo_arm_init_degree_2[2]);
  589. _arm4_init(_servo_arm_init_degree_2[3]);
  590. _arm5_init(_servo_arm_init_degree_2[4]);
  591. _arm6_init(_servo_arm_init_degree_2[5]);
  592.  
  593. _servo_arm_last_degree[0] = _servo_arm_init_degree_2[0];
  594. _servo_arm_last_degree[1] = _servo_arm_init_degree_2[1];
  595. _servo_arm_last_degree[2] = _servo_arm_init_degree_2[2];
  596. _servo_arm_last_degree[3] = _servo_arm_init_degree_2[3];
  597. _servo_arm_last_degree[4] = _servo_arm_init_degree_2[4];
  598. _servo_arm_last_degree[5] = _servo_arm_init_degree_2[5];
  599. }
  600.  
  601. void _arm_init_3() {
  602. _arm1_init(_servo_arm_init_degree_3[0]);
  603. _arm2_init(_servo_arm_init_degree_3[1]);
  604. _arm3_init(_servo_arm_init_degree_3[2]);
  605. _arm4_init(_servo_arm_init_degree_3[3]);
  606. _arm5_init(_servo_arm_init_degree_3[4]);
  607. _arm6_init(_servo_arm_init_degree_3[5]);
  608.  
  609. _servo_arm_last_degree[0] = _servo_arm_init_degree_3[0];
  610. _servo_arm_last_degree[1] = _servo_arm_init_degree_3[1];
  611. _servo_arm_last_degree[2] = _servo_arm_init_degree_3[2];
  612. _servo_arm_last_degree[3] = _servo_arm_init_degree_3[3];
  613. _servo_arm_last_degree[4] = _servo_arm_init_degree_3[4];
  614. _servo_arm_last_degree[5] = _servo_arm_init_degree_3[5];
  615. }
  616.  
  617. void _arm1_degree(int _degree, String mode) {
  618. _degree = _adjust_degree(_degree);
  619. if (mode == "sweep") {
  620. _servo_move(0, _degree, "arm", servo_arm1);
  621. }
  622. else if (mode == "quick") {
  623. _servo_quick_move(0, _degree, "arm", servo_arm1);
  624. }
  625. }
  626. void _arm2_degree(int _degree, String mode) {
  627. _degree = _adjust_degree(_degree);
  628. if (mode == "sweep") {
  629. _servo_move(1, _degree, "arm", servo_arm2);
  630. }
  631. else if (mode == "quick") {
  632. _servo_quick_move(1, _degree, "arm", servo_arm2);
  633. }
  634. }
  635. void _arm3_degree(int _degree, String mode) {
  636. _degree = _adjust_degree(_degree);
  637. if (mode == "sweep") {
  638. _servo_move(2, _degree, "arm", servo_arm3);
  639. }
  640. else if (mode == "quick") {
  641. _servo_quick_move(2, _degree, "arm", servo_arm3);
  642. }
  643. }
  644.  
  645. void _arm4_degree(int _degree, String mode) {
  646. _degree = _adjust_degree(_degree);
  647. if (mode == "sweep") {
  648. _servo_move(3, _degree, "arm", servo_arm4);
  649. }
  650. else if (mode == "quick") {
  651. _servo_quick_move(3, _degree, "arm", servo_arm4);
  652. }
  653. }
  654.  
  655. void _arm5_degree(int _degree, String mode) {
  656. _degree = _adjust_degree(_degree);
  657. if (mode == "sweep") {
  658. _servo_move(4, _degree, "arm", servo_arm5);
  659. }
  660. else if (mode == "quick") {
  661. _servo_quick_move(4, _degree, "arm", servo_arm5);
  662. }
  663. }
  664. void _arm6_degree(int _degree, String mode) {
  665. _degree = _adjust_degree(_degree);
  666. if (mode == "sweep") {
  667. _servo_move(5, _degree, "arm", servo_arm6);
  668. }
  669. else if (mode == "quick") {
  670. _servo_quick_move(5, _degree, "arm", servo_arm6);
  671. }
  672. }
  673.  
  674. void _arm_all_degree(int _degree1, int _degree2, int _degree3, int _degree4, int _degree5, int _degree6, String mode) {
  675. _arm6_degree(_degree6, mode);
  676. _delay_ms(_servo_delay_low_ms);
  677. _arm5_degree(_degree5, mode);
  678. _delay_ms(_servo_delay_low_ms);
  679. _arm4_degree(_degree4, mode);
  680. _delay_ms(_servo_delay_low_ms);
  681. _arm3_degree(_degree3, mode);
  682. _delay_ms(_servo_delay_low_ms);
  683. _arm2_degree(_degree2, mode);
  684. _delay_ms(_servo_delay_low_ms);
  685. _arm1_degree(_degree1, mode);
  686. _delay_ms(_servo_delay_low_ms);
  687. }
  688.  
  689. void _arm_all_degree_rev(int _degree1, int _degree2, int _degree3, int _degree4, int _degree5, int _degree6, String mode) {
  690. _arm1_degree(_degree6, mode);
  691. _delay_ms(_servo_delay_low_ms);
  692. _arm2_degree(_degree5, mode);
  693. _delay_ms(_servo_delay_low_ms);
  694. _arm3_degree(_degree4, mode);
  695. _delay_ms(_servo_delay_low_ms);
  696. _arm4_degree(_degree3, mode);
  697. _delay_ms(_servo_delay_low_ms);
  698. _arm5_degree(_degree2, mode);
  699. _delay_ms(_servo_delay_low_ms);
  700. _arm6_degree(_degree1, mode);
  701. _delay_ms(_servo_delay_low_ms);
  702. }
  703.  
  704.  
  705. /* ------------------------------------------------------ */
  706. /* eof arm servo operations */
  707.  
  708. /* ------------------------------------------------------ */
  709. /* command processing functions */
  710.  
  711. void _do_proc_cmd(String cmd, int _num1, int _num2, int _num3, int _num4, int _num5, int _num6) {
  712. /* arm functions */
  713. if (cmd == "a1") {
  714. _arm1_degree(_num1, "sweep");
  715. }
  716. else if (cmd == "a2") {
  717. _arm2_degree(_num2, "sweep");
  718. }
  719. else if (cmd == "a3") {
  720. _arm3_degree(_num3, "sweep");
  721. }
  722. else if (cmd == "a4") {
  723. _arm4_degree(_num4, "sweep");
  724. }
  725. else if (cmd == "a5") {
  726. _arm5_degree(_num5, "sweep");
  727. }
  728. else if (cmd == "a6") {
  729. _arm6_degree(_num6, "sweep");
  730. }
  731.  
  732. else if (cmd == "a1_quick") {
  733. _arm1_degree(_num1, "quick");
  734. }
  735. else if (cmd == "a2_quick") {
  736. _arm2_degree(_num2, "quick");
  737. }
  738. else if (cmd == "a3_quick") {
  739. _arm3_degree(_num3, "quick");
  740. }
  741. else if (cmd == "a4_quick") {
  742. _arm4_degree(_num4, "quick");
  743. }
  744. else if (cmd == "a5_quick") {
  745. _arm5_degree(_num5, "quick");
  746. }
  747. else if (cmd == "a6_quick") {
  748. _arm6_degree(_num6, "quick");
  749. }
  750.  
  751. else if (cmd == "a_init1") {
  752. _arm_init_1();
  753. }
  754. else if (cmd == "a_init2") {
  755. _arm_init_2();
  756. }
  757. else if (cmd == "a_init3") {
  758. _arm_init_3();
  759. }
  760. else if (cmd == "a_reattach") {
  761. _reattach_arm_servo();
  762. }
  763. else if (cmd == "a_redetach") {
  764. _redetach_arm_servo();
  765. }
  766. else if (cmd == "a_all") {
  767. _arm_all_degree(_num1, _num2, _num3, _num4, _num5, _num6, "sweep");
  768. }
  769. else if (cmd == "a_all_rev") {
  770. _arm_all_degree_rev(_num1, _num2, _num3, _num4, _num5, _num6, "sweep");
  771. }
  772. /* eof arm functions */
  773.  
  774. /* head functions */
  775. else if (cmd == "h_reattach") {
  776. _reattach_head_servo();
  777. }
  778. else if (cmd == "h_redetach") {
  779. _redetach_head_servo();
  780. }
  781. else if (cmd == "h1") {
  782. /*
  783. Serial.println("head 1 degree");
  784. Serial.println(_num1);
  785. */
  786. _head_joint_1_degree(_num1, "sweep");
  787. }
  788. else if (cmd == "h2") {
  789.  
  790. Serial.println("head 2 degree");
  791. Serial.println(_num1);
  792.  
  793. _head_joint_2_degree(_num1, "sweep");
  794. }
  795. else if (cmd == "h1_quick") {
  796. _head_joint_1_degree(_num1, "quick");
  797. }
  798. else if (cmd == "h2_quick") {
  799. _head_joint_2_degree(_num1, "quick");
  800. }
  801. else if (cmd == "h_all") {
  802. _head_all_degree(_num1, _num2, "sweep");
  803. }
  804. else if (cmd == "h_all_rev") {
  805. _head_all_degree_rev(_num1, _num2, "sweep");
  806. }
  807. else if (cmd == "h_init") {
  808. _head_init();
  809. }
  810. /* eof head functions */
  811.  
  812. /* mobility functions */
  813. else if (cmd == "f") {
  814. maju(_num1);
  815. }
  816. else if (cmd == "b") {
  817. mundur(_num1);
  818. }
  819. else if (cmd == "ml") {
  820. manuver_kiri(_num1);
  821. }
  822. else if (cmd == "mr") {
  823. manuver_kanan(_num1);
  824. }
  825. else if (cmd == "r") {
  826. belok_kanan(_num1);
  827. }
  828. else if (cmd == "l") {
  829. belok_kiri(_num1);
  830. }
  831. else if (cmd == "s") {
  832. _diam();
  833. }
  834. else if (cmd == "u") {
  835. /*
  836. Serial.println("_get_distances_from_sonars");
  837. */
  838. _get_distances_from_sonars(_num1);
  839. }
  840. else {
  841. _diam();
  842. }
  843. /* eof mobility functions */
  844. }
  845.  
  846. void _main(int is_this_a_thread) {
  847. String data = "";
  848. int cmd_len = 0;
  849. const char *delimeter = "|";
  850. char *pch;
  851. int _num1 = 0;
  852. int _num2 = 0;
  853. int _num3 = 0;
  854. int _num4 = 0;
  855. int _num5 = 0;
  856. int _num6 = 0;
  857. String _cmd = "";
  858. int loop = 1;
  859.  
  860.  
  861. while (loop == 1) {
  862. if (is_this_a_thread == 1) {
  863. loop = 0;
  864. }
  865. if (Serial.available() > 0) {
  866. data = Serial.readString();
  867. data.trim();
  868. cmd_len = data.length();
  869. char _buf[cmd_len + 1];
  870. data.toCharArray(_buf,cmd_len + 1);
  871. pch = strtok(_buf, delimeter);
  872. _cmd = String(pch);
  873. if (_cmd != "s") {
  874. pch = strtok(NULL, delimeter);
  875. _num1 = atoi(pch);
  876. if ( (_cmd == "h_all") or (_cmd == "h_all_rev") or (_cmd == "a_all") or (_cmd == "a_all_rev")) {
  877. pch = strtok(NULL, delimeter);
  878. _num2 = atoi(pch);
  879. if ((_cmd == "a_all") or (_cmd == "a_all_rev")) {
  880. pch = strtok(NULL, delimeter);
  881. _num3 = atoi(pch);
  882. pch = strtok(NULL, delimeter);
  883. _num4 = atoi(pch);
  884. pch = strtok(NULL, delimeter);
  885. _num5 = atoi(pch);
  886. pch = strtok(NULL, delimeter);
  887. _num6 = atoi(pch);
  888. }
  889. }
  890. }
  891. _do_proc_cmd(_cmd, _num1, _num2, _num3, _num4, _num5, _num6);
  892. }
  893. }
  894. }
  895.  
  896. /* eof command processing functions */
  897. /* ------------------------------------------------------ */
  898.  
  899. void loop() {
  900. _head_init();
  901. _main(0);
  902. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!