Components and supplies
i2C1602 LCD Display
Trim Potentiometer
IR Receiver
IR Remote
Kee Yees Nano I/O Expansion
6DOF Robot Mechanical Assembly kit - Yammis
Arduino Nano
Apps and platforms
Arduino IDE
Project description
Code
6 DOF Robot Arm C Code
c
1//www.elegoo.com 2//2018.12.19 3#include <Servo.h> 4#include <IRremote.h> 5#include <LiquidCrystal_I2C.h> 6LiquidCrystal_I2C lcd(0x27, 20, 4); 7Servo myservo1;// 8Servo myservo2;// 9Servo myservo3;// 10Servo myservo4;// 11Servo myservo5;// 12Servo myservo6;// 13 14int servoPosition; 15int analogValue; 16 17void slowServo1(int degInitial, int degFinal); 18void slowServo2(int degInitial, int degFinal); 19void slowServo3(int degInitial, int degFinal); 20void slowServo4(int degInitial, int degFinal); 21void slowServo5(int degInitial, int degFinal); 22void slowServo6(int degInitial, int degFinal); 23 24void addZero(int setNumber,int column, int row); 25 26void sine_gen1(); 27void sine_gen2(); 28void sine_gen3(); 29void sine_gen4(); 30void sine_gen5(); 31int sine1; 32int angle1; 33int sine2; 34int angle2; 35int sine3; 36int angle3; 37 38static int initial4=58;//set for horizontal scan 39static int final4=59;//set for horizontal scan 40static int initial3; 41static int final3; 42 43int analogA1 = A1; 44int analogA2 = A2; 45int analogA3 = A3; 46int analogA4 = A0;// do not use A4 - i2C 47int analogA5 = A7;// do not use A5 -i2C 48int analogA6 = A6; 49int Joint1; 50int Joint2; 51int Joint3; 52int Joint4; 53int Joint5; 54int Joint6; 55 56 57int receiver = 3; // Nano Signal Pin of IR receiver 58 59static bool execute0; 60static bool execute1; 61static bool execute2; 62static bool execute3; 63static bool execute4; 64static bool execute5; 65static bool execute5_2; 66static bool execute5_3; 67static bool execute5_4; 68static bool execute_Analog; 69static bool execute_Sine; 70static bool execute_Sweep; 71 72void translateIR(); // takes action based on IR code received 73 74void Home_Task2(); 75void Analog_Set_Task1(); 76void HomeToFive(); 77void FiveToHome(); 78void HomeToOne(); 79void OneToHome(); 80void HomeToTwo(); 81void TwoToHome(); 82void HomeToThree(); 83void ThreeToHome(); 84void HomeToFour(); 85void FourToHome(); 86void HomeToFive2(); 87void Five2ToHome(); 88void Five3ToHome(); 89void HomeToFive3(); 90void Five4ToHome(); 91void HomeToFive4(); 92void setupHome(); 93 94void horizontalScan(); 95 96IRrecv irrecv(receiver); // create instance of 'irrecv' 97decode_results results; // create instance of 'decode_results' 98void setup() 99{ 100Serial.begin(9600); 101delay(5000); 102setupHome(); 103Serial.begin(9600); 104Serial.println("IR Receiver Button Decode"); 105irrecv.enableIRIn(); // Start the receiver 106lcd.init(); 107lcd.backlight(); 108 109 110} 111void loop() 112{ 113 114//************Infra Red Remote Control****************** 115if (irrecv.decode(&results) )// have we received an IR signal? 116 { 117 translateIR(); 118 irrecv.resume(); // receive the next value 119 Serial.print("results = "); 120 Serial.println(results.value, HEX); 121 } 122 if(execute_Analog==true) 123 Analog_Set_Task1(); 124 125 if(execute_Sine==true) 126 { 127 sine_gen1(); 128 sine_gen2(); 129 sine_gen3(); 130 } 131 if(execute_Sweep==true) 132 { 133 horizontalScan(); 134 } 135} 136//************sine wave functions 137void sine_gen1() 138{ //SINE WAVE GEN 139// frequency 1/(360*delay_time) 140angle1 = angle1 + 2; if (angle1 > 360) angle1 = 0; 141//sine1 = 90+90 * sin((angle1 * PI / 180)); 142sine1 = 90+25*sin((90+angle1) * 0.0174532925); 143myservo1.write(sine1); 144//sine1 = abs(sine); 145lcd.setCursor(0,0); 146 lcd.print(" "); 147 lcd.setCursor(0,0); 148 lcd.print("1="); 149 lcd.setCursor(2,0); 150 lcd.print(angle1); 151} 152 153void sine_gen2() 154{ //SINE WAVE GEN 155// frequency 1/(360*delay_time) 156angle2 = angle2 + 2; if (angle2 > 360) angle2 = 0; 157//sine = 90+90 * sin((angle2 * PI / 180)); 158sine2 = 100+30*sin(angle2* 0.0174532925); 159myservo4.write(sine2); 160//sine2 = abs(sine); 161lcd.setCursor(6,1); 162 lcd.print(" "); 163 lcd.setCursor(6,1); 164 lcd.print("4="); 165 lcd.setCursor(8,1); 166 lcd.print(angle2); 167} 168 169void sine_gen3() 170{ //SINE WAVE GEN 171// frequency 1/(360*delay_time) 172angle3 = angle3 + 2; if (angle3 > 360) angle3 = 0; 173//sine = 90+90 * sin((angle2 * PI / 180)); 174sine3 = 90+2*sin((180+angle3)* 0.0174532925); 175myservo2.write(sine3); 176//sine2 = abs(sine); 177lcd.setCursor(6,0); 178 lcd.print(" "); 179 lcd.setCursor(6,0); 180 lcd.print("2="); 181 lcd.setCursor(8,0); 182 lcd.print(angle3); 183} 184 185void horizontalScan() 186{ 187 188int initial1=35; 189int final1=145; 190slowServo1(initial1,final1); 191 192if(final4>19) 193 { 194 initial4=initial4-10; 195 final4=final4-10; 196 slowServo4(initial4,final4); 197 final3=final4-10; 198 initial3=initial4-10; 199 } 200else 201 { 202 initial3=initial3+2; 203 final3=final3+4; 204 if(final3<30) 205 slowServo3(initial3,final3); 206 207 } 208 209initial1=145; 210final1=35; 211slowServo1(initial1,final1); 212 213if(final4>19) 214 { 215 initial4=initial4-10; 216 final4=final4-10; 217 slowServo4(initial4,final4); 218 final3=final4-10; 219 initial3=initial4-10; 220 } 221else 222 { 223 initial3=initial3+2; 224 final3=final3+4; 225 if(final3<30) 226 slowServo3(initial3,final3); 227 } 228if(final3>30 && final4<=19) 229 { 230 execute_Sweep=false; 231 slowServo1(final1, 90); 232 slowServo4(final4,108); 233 slowServo3(final3,3); 234 initial4=58; 235 final4=59; 236 } 237} 238 239//********************IR Control******************************************* 240void translateIR() // takes action based on IR code received 241// describing Remote IR codes 242{ 243 244switch(results.value) 245 { 246 case 0xFFA25D: Serial.println("POWER"); 247 setupHome(); 248 break; 249 250 case 0xFFE21D: Serial.println("FUNC/STOP"); 251 execute_Analog=true; 252 break; 253 254 case 0xFF629D: Serial.println("VOL+"); break; 255 256 break; 257 258 case 0xFF22DD: Serial.println("FAST BACK"); 259 260 break; 261 262 case 0xFF02FD: Serial.println("PAUSE"); 263 execute_Sweep=true; 264 slowServo4(108,58);// enable for horizonal scan 265 break; 266 267 case 0xFFC23D: Serial.println("FAST FORWARD"); 268 269 break; 270 271 case 0xFFE01F: Serial.println("DOWN"); 272 273 break; 274 275 case 0xFFA857: Serial.println("VOL-"); 276 277 break; 278 279 case 0xFF906F: Serial.println("UP"); 280 281 break; 282 283 case 0xFF9867: Serial.println("EQ"); 284 execute_Sine=true; 285 break; 286 287 case 0xFFB04F: Serial.println("ST/REPT"); 288 execute0=false; 289 execute1=false; 290 execute2=false; 291 execute3=false; 292 execute4=false; 293 execute5=false; 294 execute5_2=false; 295 execute5_3=false; 296 execute5_4=false; 297 execute_Analog=false; 298 execute_Sine=false; 299 execute_Sweep=false; 300 Serial.print("execute_Sweep = "); 301 Serial.print(execute_Sweep); 302 break; 303 304 case 0xFF6897: Serial.println("0"); 305 if (execute0==false ) 306 { 307 HomeToOne(); 308 delay(500); 309 OneToHome(); 310 delay(500); 311 HomeToFive(); 312 delay(500); 313 FiveToHome(); 314 delay(500); 315 316 HomeToTwo(); 317 delay(500); 318 TwoToHome(); 319 delay(500); 320 HomeToFive2(); 321 delay(500); 322 Five2ToHome(); 323 delay(500); 324 325 HomeToThree(); 326 delay(500); 327 ThreeToHome(); 328 delay(500); 329 HomeToFive3(); 330 delay(500); 331 Five3ToHome(); 332 delay(500); 333 334 335 HomeToFour(); 336 delay(500); 337 FourToHome(); 338 delay(500); 339 HomeToFive4(); 340 delay(500); 341 Five4ToHome(); 342 delay(500); 343 } 344 execute0=true; 345 break; 346 347 case 0xFF30CF: Serial.println("1"); 348 if (execute1==false ) 349 { 350 HomeToOne(); 351 delay(500); 352 OneToHome(); 353 delay(500); 354 } 355 execute1=true; 356 break; 357 358 case 0xFF18E7: Serial.println("2"); 359 if (execute2==false ) 360 { 361 HomeToTwo(); 362 delay(500); 363 TwoToHome(); 364 delay(500); 365 } 366 break; 367 execute2=true; 368 369 case 0xFF7A85: Serial.println("3"); 370 if (execute3==false ) 371 { 372 HomeToThree(); 373 delay(500); 374 ThreeToHome(); 375 delay(500); 376 } 377 execute3=true; 378 break; 379 case 0xFF10EF: Serial.println("4"); 380 if (execute4==false ) 381 { 382 HomeToFour(); 383 delay(500); 384 FourToHome(); 385 delay(500); 386 } 387 execute4=true; 388 break; 389 390 case 0xFF38C7: Serial.println("5"); 391 if (execute5==false ) 392 { 393 HomeToFive(); 394 delay(500); 395 FiveToHome(); 396 delay(500); 397 } 398 execute5=true; 399 break; 400 401 case 0xFF5AA5: Serial.println("6 or 5_2"); 402 if (execute5_2==false ) 403 { 404 HomeToFive2(); 405 delay(500); 406 Five2ToHome(); 407 delay(500); 408 } 409 execute5_2=true; 410 break; 411 412 case 0xFF42BD: Serial.println("7 or 5_3"); 413 if (execute5_3==false ) 414 { 415 HomeToFive3(); 416 delay(500); 417 Five3ToHome(); 418 delay(500); 419 } 420 execute5_3=true; 421 break; 422 423 424 case 0xFF4AB5: Serial.println("8 or 5_4"); 425 if (execute5_4==false ) 426 { 427 HomeToFive4(); 428 delay(500); 429 Five4ToHome(); 430 delay(500); 431 } 432 execute5_4=true; 433 break; 434 435 case 0xFF52AD: Serial.println("9 reset execute bools to false"); 436 437 break; 438 439 case 0xFFFFFFFF: Serial.println(" REPEAT"); 440 441 break; 442 443 default: 444 Serial.println(" other button "); 445 }// End Case 446 447} //END translateIR 448 449void HomeToOne() 450{ 451int i; 452 453 slowServo3(0,45); 454 Home_Task2(); 455 slowServo1(92,115); 456 delay(500); 457 slowServo2(85,97); 458 delay(500); 459 slowServo3(45,21);// Added to Raise Arm 460 delay(500); 461 slowServo4(108,92); 462 delay(500); 463 slowServo5(80,108); 464 delay(500); 465 slowServo6(152,120); 466 delay(500); 467 slowServo3(21,11); 468 delay(500); 469 slowServo6(120,152); //close J5 470 delay(500); 471 472 473} 474 475void OneToHome() 476{ 477 slowServo3(11,45);// changed from (12,21) 478 delay(500); 479 slowServo6(152,152); 480 delay(500); 481 slowServo5(108,80); 482 delay(500); 483 slowServo4(92,108); 484 delay(500); 485 slowServo2(97,85); 486 delay(500); 487 slowServo1(115,92); 488 delay(500); 489 Home_Task2(); 490 slowServo3(45,0);// Added to lower Arm 491 delay(500); 492 493} 494void HomeToTwo() 495{ 496int i; 497 498 slowServo3(0,45); 499 Home_Task2(); 500 slowServo1(92,101); 501 delay(500); 502 slowServo2(85,94); 503 delay(500); 504 slowServo3(45,21);// Added to Raise Arm 505 delay(500); 506 slowServo4(108,92); 507 delay(500); 508 slowServo5(80,91); 509 delay(500); 510 slowServo6(152,120); 511 delay(500); 512 slowServo3(21,8); 513 delay(500); 514 slowServo6(120,152); 515 delay(500); 516 517} 518 519void TwoToHome() 520{ 521slowServo3(8,21); 522 delay(500); 523slowServo6(152,152); 524 delay(500); 525slowServo5(91,80); 526 delay(500); 527slowServo4(92,108); 528 delay(500); 529slowServo3(21,45);// Added to Raise Arm 530 delay(500); 531slowServo2(94,85); 532 delay(500); 533slowServo1(101,92); 534 delay(500); 535Home_Task2(); 536delay(500); 537slowServo3(45,0); 538 539} 540 541 542void HomeToThree() 543{ 544 545 slowServo3(0,45); 546 Home_Task2(); 547 slowServo1(92,83); 548 delay(500); 549 slowServo2(85,97); 550 delay(500); 551 slowServo3(45,23);// Added to Raise Arm 552 delay(500); 553 slowServo4(108,92); 554 delay(500); 555 slowServo5(80,79); 556 delay(500); 557 slowServo6(152,120); 558 delay(500); 559 slowServo3(23,10); 560 delay(500); 561 slowServo6(120,152); 562 delay(500); 563 564} 565 566void ThreeToHome() 567{ 568 569 slowServo3(10,23); 570 delay(500); 571 slowServo6(152,152); 572 delay(500); 573 slowServo5(79,80); 574 delay(500); 575 slowServo4(92,108); 576 delay(500); 577 slowServo3(23,45);// Added to Raise Arm 578 delay(500); 579 slowServo2(97,85); 580 delay(500); 581 slowServo1(83,92); 582 delay(500); 583 Home_Task2(); 584 slowServo3(45,0); 585 586} 587 588 589void HomeToFour() 590{ 591 592 slowServo3(0,45); 593 Home_Task2(); 594 slowServo1(92,69); 595 delay(500); 596 slowServo2(85,97); 597 delay(500); 598 slowServo3(45,23);// Added to Raise Arm 599 delay(500); 600 slowServo4(108,92); 601 delay(500); 602 slowServo5(80,64); 603 delay(500); 604 slowServo6(152,120); 605 delay(500); 606 slowServo3(23,12); 607 slowServo6(120,152); 608 delay(500); 609 610} 611 612void FourToHome() 613{ 614 615 slowServo3(12,23); 616 delay(500); 617 slowServo6(152,152); 618 delay(500); 619 slowServo5(64,80); 620 delay(500); 621 slowServo4(92,108); 622 delay(500); 623 slowServo3(23,45);// Added to Raise Arm 624 delay(500); 625 slowServo2(97,85); 626 delay(500); 627 slowServo1(69,92); 628 delay(500); 629 Home_Task2(); 630 delay(500); 631 slowServo3(45,0); 632 633} 634 635 636void HomeToFive() 637{ 638 639 slowServo3(0,45); 640 Home_Task2(); 641 slowServo1(92,92); 642 delay(500); 643 slowServo2(85,100); 644 delay(500); 645 slowServo3(45,28);// Added to Raise Arm 646 delay(500); 647 slowServo4(108,67); 648 delay(500); 649 slowServo5(80,80); 650 delay(500); 651 slowServo3(28,8); 652 delay(500); 653 slowServo6(152,120); 654 delay(500); 655 656} 657 658void FiveToHome() 659{ 660 661slowServo3(8,28); 662 delay(500); 663slowServo6(120,152); 664 delay(500); 665slowServo5(80,80); 666 delay(500); 667slowServo4(67,108); 668 delay(500); 669 slowServo3(28,45);// Added to Raise Arm 670 delay(500); 671 slowServo2(100,85); 672 delay(500); 673 slowServo1(92,92); 674 delay(500); 675 Home_Task2(); 676 slowServo3(45,0); 677 678} 679 680void HomeToFive2() 681{ 682 683 slowServo3(0,45); 684 Home_Task2(); 685 slowServo1(92,92); 686 delay(500); 687 slowServo2(85,100); 688 delay(500); 689 slowServo3(45,30);// Added to Raise Arm 690 delay(500); 691 slowServo4(108,78); 692 delay(500); 693 slowServo5(80,80); 694 delay(500); 695 slowServo3(30,19); 696 delay(500); 697 slowServo6(152,120); 698 delay(500); 699 700} 701 702void Five2ToHome() 703{ 704slowServo3(19,30); 705 delay(500); 706slowServo6(120,152); 707 delay(500); 708slowServo5(80,80); 709 delay(500); 710slowServo4(78,108); 711 delay(500); 712 slowServo3(30,45);// Added to Raise Arm 713 delay(500); 714 slowServo2(100,85); 715 delay(500); 716 slowServo1(92,92); 717 delay(500); 718 Home_Task2(); 719 slowServo3(45,0); 720} 721 722 723void HomeToFive3() 724{ 725 726 slowServo3(0,45); 727 Home_Task2(); 728 slowServo1(92,92); 729 delay(500); 730 slowServo2(85,97); 731 delay(500); 732 slowServo3(45,40);// Added to Raise Arm 733 delay(500); 734 slowServo4(108,76); 735 delay(500); 736 slowServo5(80,80); 737 delay(500); 738 slowServo3(40,16); 739 delay(500); 740 slowServo6(152,120); 741 delay(500); 742 743} 744 745void Five3ToHome() 746{ 747slowServo3(16,40); 748 delay(500); 749slowServo6(120,152); 750 delay(500); 751slowServo5(80,80); 752 delay(500); 753slowServo4(76,108); 754 delay(500); 755 slowServo3(40,45);// Added to Raise Arm 756 delay(500); 757 slowServo2(97,85); 758 delay(500); 759 slowServo1(92,92); 760 delay(500); 761 Home_Task2(); 762 slowServo3(45,0); 763} 764 765 766void HomeToFive4() 767{ 768 769 slowServo3(0,45); 770 Home_Task2(); 771 slowServo1(92,92); 772 delay(500); 773 slowServo2(85,91); 774 delay(500); 775 slowServo3(45,43);// Added to Raise Arm 776 delay(500); 777 slowServo4(108,76); 778 delay(500); 779 slowServo5(80,80); 780 delay(500); 781 slowServo3(43,14); 782 delay(500); 783 slowServo6(152,120); 784 delay(500); 785 786} 787 788void Five4ToHome() 789{ 790slowServo3(14,43); 791 delay(500); 792slowServo6(120,152); 793 delay(500); 794slowServo5(80,80); 795 delay(500); 796slowServo4(76,108); 797 delay(500); 798 slowServo3(43,45);// Added to Raise Arm 799 delay(500); 800 slowServo2(91,85); 801 delay(500); 802 slowServo1(92,92); 803 delay(500); 804 Home_Task2(); 805 slowServo3(45,0); 806} 807//*******************Home Position*********************** 808 809 810void Home_Task2() 811{ 812 813 814 myservo1.write(92); // Joint 1 89 (home) 815 myservo2.write(85); // Joint 2 107 (home) 816 myservo3.write(45); // Joint 3 24 (home) 817 myservo4.write(108); // Joint 82 (home) 818 //myservo4.write(92); // Joint 82 (home) 819 myservo5.write(80); // Joint 5 84 (home) 820 myservo6.write(152); // Joint 6 1666 (home) closed 821} 822 823void setupHome() 824{ 825 myservo1.attach(9);//Joint 1 826 myservo1.write(92); // Joint 1 89 (home) 827 delay(500); 828 myservo2.attach(8);//Joint 2 829 myservo2.write(85); // Joint 2 107 (home) 830 delay(500); 831 myservo3.attach(7);//Joint 3 832 myservo3.write(3); 833 //slowServo3(4,90); 834 delay(500); 835 //slowServo3(0,45); 836 myservo4.attach(6);// Joint 4 837 myservo4.write(108); 838 //slowServo4(108,58);// enable for horizonal scan 839 delay(500); 840 myservo5.attach(5);// Joint 5 841 myservo5.write(80); // Joint 5 84 (home) 842 delay(500); 843 myservo6.attach(4); // Joint 6 844 myservo6.write(163); // Joint 6 1666 (home) closed 845 delay(500); 846} 847 848 849//*****Servo Movement Controls*************************** 850void slowServo1(int degInitial, int degFinal) 851{ 852int i; 853 854if(degInitial<degFinal) 855 { 856 for (i=degInitial; i<degFinal; i++) 857 { 858 delay(50); 859 myservo1.write(i); 860 } 861 } 862else 863 { 864 for (i=degInitial; i>degFinal; i--) 865 { 866 delay(50); 867 myservo1.write(i); 868 } 869 } 870 lcd.setCursor(0,0); 871 lcd.print(" "); 872 lcd.setCursor(0,0); 873 lcd.print("1="); 874 lcd.setCursor(2,0); 875 lcd.print(i); 876} 877void slowServo2(int degInitial, int degFinal) 878{ 879int i; 880if(degInitial<degFinal) 881 { 882 for (i=degInitial; i<degFinal; i++) 883 { 884 delay(100); 885 myservo2.write(i); 886 } 887 } 888else 889 { 890 for (i=degInitial; i>degFinal; i--) 891 { 892 delay(50); 893 myservo2.write(i); 894 } 895 } 896 lcd.setCursor(6,0); 897 lcd.print(" "); 898 lcd.setCursor(6,0); 899 lcd.print("2="); 900 lcd.setCursor(8,0); 901 lcd.print(i); 902} 903 904 905 906void slowServo3(int degInitial, int degFinal) 907{ 908int i; 909if(degInitial<degFinal) 910 { 911 for (i=degInitial; i<degFinal; i++) 912 { 913 delay(50); 914 myservo3.write(i); 915 } 916 } 917else 918 { 919 for (i=degInitial; i>degFinal; i--) 920 { 921 delay(50); 922 myservo3.write(i); 923 } 924 } 925 lcd.setCursor(0,1); 926 lcd.print(" "); 927 lcd.setCursor(0,1); 928 lcd.print("3="); 929 lcd.setCursor(2,1); 930 lcd.print(i); 931} 932 933void slowServo4(int degInitial, int degFinal) 934{ 935int i; 936if(degInitial<degFinal) 937 { 938 for (i=degInitial; i<degFinal; i++) 939 { 940 delay(50); 941 myservo4.write(i); 942 } 943 } 944else 945 { 946 for (i=degInitial; i>degFinal; i--) 947 { 948 delay(50); 949 myservo4.write(i); 950 } 951 } 952 lcd.setCursor(6,1); 953 lcd.print(" "); 954 lcd.setCursor(6,1); 955 lcd.print("4="); 956 lcd.setCursor(8,1); 957 lcd.print(i); 958 959} 960 961void slowServo5(int degInitial, int degFinal) 962{ 963int i; 964if(degInitial<degFinal) 965 { 966 for (i=degInitial; i<degFinal; i++) 967 { 968 delay(50); 969 myservo5.write(i); 970 } 971 } 972else 973 { 974 for (i=degInitial; i>degFinal; i--) 975 { 976 delay(50); 977 myservo5.write(i); 978 } 979 } 980 lcd.setCursor(0,0); 981 lcd.print(" "); 982 lcd.setCursor(0,0); 983 lcd.print("5="); 984 lcd.setCursor(2,0); 985 lcd.print(i); 986} 987void slowServo6(int degInitial, int degFinal) 988{ 989int i; 990if(degInitial<degFinal) 991 { 992 for (i=degInitial; i<degFinal; i++) 993 { 994 delay(50); 995 myservo6.write(i); 996 } 997 } 998else 999 { 1000 for (i=degInitial; i>degFinal; i--) 1001 { 1002 delay(50); 1003 myservo6.write(i); 1004 } 1005 } 1006 if (i>150) 1007 { 1008 lcd.setCursor(13,0); 1009 lcd.print(" "); 1010 lcd.setCursor(13,0); 1011 lcd.print("6="); 1012 lcd.setCursor(15,0); 1013 lcd.print("C"); 1014 } 1015 else if(i<125) 1016 { 1017 lcd.setCursor(13,0); 1018 lcd.print(" "); 1019 lcd.setCursor(13,0); 1020 lcd.print("6="); 1021 lcd.setCursor(15,0); 1022 lcd.print("O"); 1023 } 1024} 1025 1026 1027//**********Sets Fine Adjustment Using Trim Pots************** 1028 1029 1030void Analog_Set_Task1() 1031{ 1032 //***************Home Position*************************** 1033 //J1=90, J2=85, J3=0, J4=103, J5=83, J6=162 (closed) 1034 1035 1036 analogA1=analogRead(A1); 1037 Joint1= map(analogA1, 0, 1023, 0, 180); 1038 myservo1.write(Joint1); 1039 lcd.setCursor(0,0); 1040 lcd.print(" "); 1041 lcd.setCursor(0,0); 1042 lcd.print("1="); 1043 lcd.setCursor(2,0); 1044 lcd.print(Joint1); 1045 1046 analogA2=analogRead(A2); 1047 Joint2= map(analogA2, 0, 1023, 0, 180); 1048 myservo2.write(Joint2); 1049 lcd.setCursor(6,0); 1050 lcd.print(" "); 1051 lcd.setCursor(6,0); 1052 lcd.print("2="); 1053 lcd.setCursor(8,0); 1054 lcd.print(Joint2); 1055 1056 analogA3=analogRead(A3); 1057 Joint3= map(analogA3, 0, 1023, 0, 180); 1058 myservo3.write(Joint3); 1059 lcd.setCursor(0,1); 1060 lcd.print(" "); 1061 lcd.setCursor(0,1); 1062 lcd.print("3="); 1063 lcd.setCursor(2,1); 1064 lcd.print(Joint3); 1065 1066 analogA4=analogRead(A0); 1067 Joint4= map(analogA4, 0, 1023, 0, 180); 1068 myservo4.write(Joint4); 1069 lcd.setCursor(6,1); 1070 lcd.print(" "); 1071 lcd.setCursor(6,1); 1072 lcd.print("4="); 1073 lcd.setCursor(8,1); 1074 lcd.print(Joint4); 1075 1076 analogA5=analogRead(A7); 1077 Joint5= map(analogA5, 0, 1023, 0, 180); 1078 myservo5.write(Joint5); 1079 lcd.setCursor(13,0); 1080 lcd.print(" "); 1081 lcd.setCursor(13,0); 1082 lcd.print(Joint5); 1083 1084 analogA6=analogRead(A6); 1085 Joint6= map(analogA6, 0, 1023, 0, 180); 1086 myservo6.write(Joint6); 1087} 1088//**********Display Requirement*********************** 1089void addZero(int setNumber,int column, int row) 1090{ 1091lcd.setCursor(column, row); 1092 1093if (setNumber >= 0 && setNumber < 10) 1094 { 1095 lcd.print("0"); 1096 lcd.setCursor(column+1,row); 1097 lcd.print(setNumber); 1098 } 1099 else 1100 { 1101 lcd.print(setNumber); 1102 } 1103}
Comments
Only logged in users can leave comments
lenfromtoronto
0 Followers
•0 Projects
1
0