6 Degrees Of Freedom Robot Arm System

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}