Robotic arm 4 servos

1#include <Servo.h>
2#include <Keypad.h>
3#include <LiquidCrystal_I2C.h>
4
5LiquidCrystal_I2C  lcd(0x27,16,2);
6
7
8Servo servoA; //Hand (open/close)
9Servo servoB; //  forward Backward 
10Servo servoC; //up down
11Servo servoD; // Rotate (left-right)
12
13
14//  The kepad
15const byte ROWS = 4; 
16const byte COLS = 4;
17char keys[ROWS][COLS]  = {
18    
19    {'1','2','3','A'}, 
20    {'4','5','6','B'}, 
21    {'7','8','9','C'},
22    {'*','0','#','D'}};
23
24byte rowPins[ROWS] = { 9, 8, 7, 13 };
25byte colPins[COLS]  = { 12, 4, 3, 2 };
26Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins,  ROWS, COLS ); 
27String command;
28
29
30char leter ='A'; // (Leter) servo to  perform the command 
31int angle = 0;  // (angle) Initial command
32
33void setup()
34{
35//  initialize the lcd  
36lcd.init();                      
37lcd.backlight();
38lcd.setCursor(0,0);
39lcd.print("Enter  command"); 
40// Pins for the servos
41servoA.attach(5);
42servoB.attach(6);
43servoC.attach(10);
44servoD.attach(11);
45
46//Starting  point 
47servoA.write(80);
48servoB.write(30);
49servoC.write(120);
50servoD.write(90);
51
52
53
54
55}
56
57void  loop(){
58 
59char key = keypad.getKey();
60
61
62  if (key){
63            lcd.init();
64            lcd.setCursor(0,0);
65            lcd.print(String("Servo ")  + String(leter));
66            lcd.setCursor(0,1); 
67            lcd.print(String("Command  ")+String(command  + key));
68
69// * on the keyapd will reset the command
70              if(key  == '*') {
71                      command = ""; 
72                      lcd.init();
73                      lcd.setCursor(0,0);
74                      lcd.print(String("Servo  ")  + String(leter)); 
75                      lcd.setCursor(0,1); 
76                      lcd.print("Command  ");
77                      } 
78//# on the keypad will execute the command  on the chosen servo defined by (leter)                     
79              else  if(key == '#') 
80                    {
81                        
82                                        lcd.init();                      // initialize the lcd
83                                        lcd.setCursor(0,0);
84                                        lcd.print(String("Servo ")  + String(leter));  // show on the LCD the servo that is chosen
85                                        lcd.setCursor(0,1);
86                                        lcd.print(String("Execute ")+String(command));  // show on the LCD the servo the comand is performing
87                                        int  i = command.toInt(); // capture on (i) the integer of the command (only numbers)
88          
89                             // For SERVO A
90                              if(leter  == 'A') {
91                                                angle =servoA.read();
92                                                if ( i >= angle) {
93                                                              for  (angle = angle; angle <= i; angle=angle +1)
94                                                               {servoA.write(angle);                             
95                                                               delay(10);                                                                              
96                                                                }
97                                                                }
98                                                else 
99                                                              {  for (angle = angle; angle >= i; angle=angle- 1)
100                                                               {servoA.write(angle);
101                                                               delay(10);                                                                                                                                                     
102                                                                }              
103                                                                }
104                                                                
105                                           }
106                               //For  SERVO A
107                              if(leter == 'B') {
108                                                angle  =servoB.read();
109                                                if ( i >= angle)  {
110                                                              for (angle =  angle; angle <= i; angle=angle +1)
111                                                               {servoB.write(angle);
112                                                               delay(10);                                                                              
113                                                                }
114                                                                }
115                                                else 
116                                                              {  for (angle = angle; angle >= i; angle=angle- 1)
117                                                               {servoB.write(angle);
118                                                               delay(10);                                                                                                                                                    
119                                                                }              
120                                                                }                                                                
121                                           }
122                             
123                               // For  SERVO C
124                              if(leter  == 'C') {
125                                                angle =servoA.read();
126                                                if ( i >= angle) {
127                                                              for  (angle = angle; angle <= i; angle=angle +1)
128                                                               {servoC.write(angle);
129                                                               delay(10);
130                                                                              
131                                                                }
132                                                                }
133                                                else 
134                                                              {  for (angle = angle; angle >= i; angle=angle- 1)
135                                                               {servoC.write(angle);
136                                                                delay(10);                                                                                                                                               
137                                                                }              
138                                                                }
139                                                                
140                                           }
141                               //  For  SERVO D
142                              if(leter == 'D'){
143                                                angle  =servoA.read();
144                                                if ( i >= angle)  {
145                                                              for (angle =  angle; angle <= i; angle=angle +1)
146                                                               {servoD.write(angle);
147                                                               delay(20);
148                                                                              
149                                                                }
150                                                                }
151                                                else 
152                                                              {  for (angle = angle; angle >= i; angle=angle- 1)
153                                                               {servoD.write(angle);
154                                                                delay(20);                                                                                                                                                       
155                                                                }                                                                              }
156                                                                
157                                           }                                               
158                              lcd.init();  
159                              lcd.setCursor(0,0);
160                              lcd.print(String("Servo  ")  + String(leter)); //write on the screen the servo chosen
161                              command  = "";// empty command 
162                              }
163              //  Chosing the servo        
164              else if(key == 'A' or key == 'B' or key  == 'C' or key == 'D') 
165                               {
166                                  leter  = key;
167                                  lcd.init(); 
168                                  lcd.setCursor(0,0);
169                                  lcd.print(String("Servo ")  + String(leter));//print  the name of the servo chosen  
170                               if(leter == 'A')  {
171                                                
172                                                lcd.println(servoA.read());    //print the value from the servo chosen           
173                                                }
174                               //  SERVO A
175                              if(leter  == 'B') {
176                                                lcd.println(servoB.read());    //print the value from the servo chosen  
177                                               }
178                             
179                               //  SERVO C
180                              if(leter == 'C') {                                       
181                                                lcd.println(servoC.read());   //print  the value from the servo chosen
182                                                }
183                               //  SERVO D
184                              if(leter  == 'D') {
185                                                lcd.println(servoD.read());   //print the value of the servo chosen
186                                               }
187                                  lcd.setCursor(0,1); 
188                                  lcd.print(String("Command  ")+String(command)); //print the value of the command                 
189                                  }
190
191             
192               else {
193                command += key; //Add  a digit to the command  
194               
195              }
196  }}