Tobor, the Arm

1/*
2 * 6/2/19Versao que funcionou com o control box. apos muitos problemas  de mau contato e qualidade inferior
3 * dos servos azuis. Trocados por Towerpro  MG90S engrenagem metal.
4 * iniciar montagem do braco MeArm
5 */
6
7#include  <Wire.h>
8#include <Servo.h>
9#include <LiquidCrystal_I2C.h> // For 20x4 lcd
10LiquidCrystal_I2C  lcd(0x27,20,4);
11
12Servo s1;  // create servo object to control a servo
13Servo  s2;
14Servo s3;
15Servo s4;
16
17int potpin1 = A0;  // analog pin used to connect  the potentiometer
18int potpin2 = A1;  // analog pin used to connect the potentiometer
19int  potpin3 = A2;  // analog pin used to connect the potentiometer
20int potpin4 =  A3;  // analog pin used to connect the potentiometer
21
22int valA = 0;    //  read the value from the analog pin
23int valB = 0;    // read the value from the  analog pin
24int valC = 0;    // read the value from the analog pin
25int valD  = 0;    // read the value from the analog pin
26
27int greenButton = 12; //record  mem index and servo angles
28int redButton = 4; //reserved for future use
29int  blackButton = 7; //mem index selector 
30int switchAM = 8; //switch between Manual  or Automatic operation
31int counter = 0; //mem index
32int mTime = 500;//time  to move each servo in Aumtomatic mode one by one
33
34//for now this is the way  I found to store 10 * 4servo positions.
35//An array of a mem counter and angles  of 4 servos
36int memP01[5], memP02[5], memP03[5], memP04[5], memP05[5], memP06[5],  memP07[5], memP08[5], memP09[5], memP10[5];
37
38//move ARM according to 4 angles  stored in each Array 
39void moveCounter1() { 
40s1.write(memP01[1]); delay(mTime);
41s2.write(memP01[2]);  delay(mTime);
42s3.write(memP01[3]); delay(mTime);
43s4.write(memP01[4]); delay(mTime);
44}
45
46void  moveCounter2() {
47s1.write(memP01[1]); delay(mTime);
48s2.write(memP01[2]); delay(mTime);
49s3.write(memP01[3]);  delay(mTime);
50s4.write(memP01[4]); delay(mTime);
51s1.write(memP02[1]); delay(mTime);
52s2.write(memP02[2]);  delay(mTime);
53s3.write(memP02[3]); delay(mTime);
54s4.write(memP02[4]); delay(mTime);
55}
56
57void  moveCounter3() {
58s1.write(memP01[1]); delay(mTime);
59s2.write(memP01[2]); delay(mTime);
60s3.write(memP01[3]);  delay(mTime);
61s4.write(memP01[4]); delay(mTime);
62s1.write(memP02[1]); delay(mTime);
63s2.write(memP02[2]);  delay(mTime);
64s3.write(memP02[3]); delay(mTime);
65s4.write(memP02[4]); delay(mTime);
66s1.write(memP03[1]);  delay(mTime); 
67s2.write(memP03[2]); delay(mTime);
68s3.write(memP03[3]); delay(mTime);
69s4.write(memP03[4]);  delay(mTime);
70}
71
72void moveCounter4() {
73s1.write(memP01[1]); delay(mTime);
74s2.write(memP01[2]);  delay(mTime);
75s3.write(memP01[3]); delay(mTime);
76s4.write(memP01[4]); delay(mTime);
77s1.write(memP02[1]);  delay(mTime);
78s2.write(memP02[2]); delay(mTime);
79s3.write(memP02[3]); delay(mTime);
80s4.write(memP02[4]);  delay(mTime);
81s1.write(memP03[1]); delay(mTime);
82s2.write(memP03[2]); delay(mTime);  
83s3.write(memP03[3]); delay(mTime);
84s4.write(memP03[4]); delay(mTime);
85s1.write(memP04[1]);  delay(mTime);
86s2.write(memP04[2]); delay(mTime);
87s3.write(memP04[3]); delay(mTime);
88s4.write(memP04[4]);  delay(mTime);
89}
90
91void moveCounter5() {
92s1.write(memP01[1]); delay(mTime);
93s2.write(memP01[2]);  delay(mTime); 
94s3.write(memP01[3]); delay(mTime);
95s4.write(memP01[4]); delay(mTime);
96s1.write(memP02[1]);  delay(mTime); 
97s2.write(memP02[2]); delay(mTime); 
98s3.write(memP02[3]); delay(mTime);
99s4.write(memP02[4]);  delay(mTime);
100s1.write(memP03[1]); delay(mTime);
101s2.write(memP03[2]); delay(mTime);  
102s3.write(memP03[3]); delay(mTime);
103s4.write(memP03[4]); delay(mTime);
104s1.write(memP04[1]);  delay(mTime); 
105s2.write(memP04[2]); delay(mTime);
106s3.write(memP04[3]); delay(mTime);
107s4.write(memP04[4]);  delay(mTime);
108s1.write(memP05[1]); delay(mTime); 
109s2.write(memP05[2]); delay(mTime);
110s3.write(memP05[3]);  delay(mTime);
111s4.write(memP05[4]); delay(mTime);
112}
113
114void moveCounter6()  {
115s1.write(memP01[1]); delay(mTime);
116s2.write(memP01[2]); delay(mTime);
117s3.write(memP01[3]);  delay(mTime);
118s4.write(memP01[4]); delay(mTime);
119s1.write(memP02[1]); delay(mTime);
120s2.write(memP02[2]);  delay(mTime);
121s3.write(memP02[3]); delay(mTime);
122s4.write(memP02[4]); delay(mTime);
123s1.write(memP03[1]);  delay(mTime);
124s2.write(memP03[2]); delay(mTime);
125s3.write(memP03[3]); delay(mTime);
126s4.write(memP03[4]);  delay(mTime);
127s1.write(memP04[1]); delay(mTime); 
128s2.write(memP04[2]); delay(mTime);
129s3.write(memP04[3]);  delay(mTime);
130s4.write(memP04[4]); delay(mTime);
131s1.write(memP05[1]); delay(mTime);
132s2.write(memP05[2]);  delay(mTime); 
133s3.write(memP05[3]); delay(mTime);
134s4.write(memP05[4]); delay(mTime);
135s1.write(memP06[1]);  delay(mTime); 
136s2.write(memP06[2]); delay(mTime); 
137s3.write(memP06[3]); delay(mTime);
138s4.write(memP06[4]);  delay(mTime);
139}
140
141void moveCounter7() {
142s1.write(memP01[1]); delay(mTime);
143s2.write(memP01[2]);  delay(mTime);
144s3.write(memP01[3]); delay(mTime);
145s4.write(memP01[4]); delay(mTime);
146s1.write(memP02[1]);  delay(mTime);
147s2.write(memP02[2]); delay(mTime);
148s3.write(memP02[3]); delay(mTime);
149s4.write(memP02[4]);  delay(mTime);
150s1.write(memP03[1]); delay(mTime);
151s2.write(memP03[2]); delay(mTime);  
152s3.write(memP03[3]); delay(mTime);
153s4.write(memP03[4]); delay(mTime);
154s1.write(memP04[1]);  delay(mTime);
155s2.write(memP04[2]); delay(mTime); 
156s3.write(memP04[3]); delay(mTime);
157s4.write(memP04[4]);  delay(mTime);
158s1.write(memP05[1]); delay(mTime);
159s2.write(memP05[2]); delay(mTime);  
160s3.write(memP05[3]); delay(mTime);
161s4.write(memP05[4]); delay(mTime);
162s1.write(memP06[1]);  delay(mTime); 
163s2.write(memP06[2]); delay(mTime);
164s3.write(memP06[3]); delay(mTime);
165s4.write(memP06[4]);  delay(mTime);
166s1.write(memP07[1]); delay(mTime);
167s2.write(memP07[2]); delay(mTime);
168s3.write(memP07[3]);  delay(mTime);
169s4.write(memP07[4]); delay(mTime);
170}
171
172void moveCounter8()  {
173s1.write(memP01[1]); delay(mTime);
174s2.write(memP01[2]); delay(mTime);
175s3.write(memP01[3]);  delay(mTime);
176s4.write(memP01[4]); delay(mTime);
177s1.write(memP02[1]); delay(mTime);  
178s2.write(memP02[2]); delay(mTime);
179s3.write(memP02[3]); delay(mTime);
180s4.write(memP02[4]);  delay(mTime);
181s1.write(memP03[1]); delay(mTime);
182s2.write(memP03[2]); delay(mTime);
183s3.write(memP03[3]);  delay(mTime);
184s4.write(memP03[4]); delay(mTime);
185s1.write(memP04[1]); delay(mTime);  
186s2.write(memP04[2]); delay(mTime); 
187s3.write(memP04[3]); delay(mTime);
188s4.write(memP04[4]);  delay(mTime);
189s1.write(memP05[1]); delay(mTime);
190s2.write(memP05[2]); delay(mTime);  
191s3.write(memP05[3]); delay(mTime);
192s4.write(memP05[4]); delay(mTime);
193s1.write(memP06[1]);  delay(mTime); 
194s2.write(memP06[2]); delay(mTime); 
195s3.write(memP06[3]); delay(mTime);
196s4.write(memP06[4]);  delay(mTime);
197s1.write(memP07[1]); delay(mTime);
198s2.write(memP07[2]); delay(mTime);  
199s3.write(memP07[3]); delay(mTime);
200s4.write(memP07[4]); delay(mTime);
201s1.write(memP08[1]);  delay(mTime);
202s2.write(memP08[2]); delay(mTime); 
203s3.write(memP08[3]); delay(mTime);
204s4.write(memP08[4]);  delay(mTime);
205}
206
207void moveCounter9() {
208s1.write(memP01[1]); delay(mTime);
209s2.write(memP01[2]);  delay(mTime);
210s3.write(memP01[3]); delay(mTime); 
211s4.write(memP01[4]); delay(mTime);
212s1.write(memP02[1]);  delay(mTime);
213s2.write(memP02[2]); delay(mTime); 
214s3.write(memP02[3]); delay(mTime);  
215s4.write(memP02[4]); delay(mTime);
216s1.write(memP03[1]); delay(mTime);
217s2.write(memP03[2]);  delay(mTime); 
218s3.write(memP03[3]); delay(mTime); 
219s4.write(memP03[4]); delay(mTime);
220s1.write(memP04[1]);  delay(mTime);
221s2.write(memP04[2]); delay(mTime); 
222s3.write(memP04[3]); delay(mTime);
223s4.write(memP04[4]);  delay(mTime);
224s1.write(memP05[1]); delay(mTime);
225s2.write(memP05[2]); delay(mTime);  
226s3.write(memP05[3]); delay(mTime);
227s4.write(memP05[4]); delay(mTime);
228s1.write(memP06[1]);  delay(mTime); 
229s2.write(memP06[2]); delay(mTime);
230s3.write(memP06[3]); delay(mTime);  
231s4.write(memP06[4]); delay(mTime);
232s1.write(memP07[1]); delay(mTime);
233s2.write(memP07[2]);  delay(mTime);
234s3.write(memP07[3]); delay(mTime); 
235s4.write(memP07[4]); delay(mTime);
236s1.write(memP08[1]);  delay(mTime);
237s2.write(memP08[2]); delay(mTime);
238s3.write(memP08[3]); delay(mTime);  
239s4.write(memP08[4]); delay(mTime);
240s1.write(memP09[1]); delay(mTime); 
241s2.write(memP09[2]);  delay(mTime); 
242s3.write(memP09[3]); delay(mTime);
243s4.write(memP09[4]); delay(mTime);
244}
245
246void  moveCounter10() {
247s1.write(memP01[1]); delay(mTime); 
248s2.write(memP01[2]);  delay(mTime);
249s3.write(memP01[3]); delay(mTime);
250s4.write(memP01[4]); delay(mTime);
251s1.write(memP02[1]);  delay(mTime);
252s2.write(memP02[2]); delay(mTime);
253s3.write(memP02[3]); delay(mTime);
254s4.write(memP02[4]);  delay(mTime);
255s1.write(memP03[1]); delay(mTime);
256s2.write(memP03[2]); delay(mTime);
257s3.write(memP03[3]);  delay(mTime);
258s4.write(memP03[4]); delay(mTime);
259s1.write(memP04[1]); delay(mTime);
260s2.write(memP04[2]);  delay(mTime); 
261s3.write(memP04[3]); delay(mTime);
262s4.write(memP04[4]); delay(mTime);
263s1.write(memP05[1]);  delay(mTime);
264s2.write(memP05[2]); delay(mTime);
265s3.write(memP05[3]); delay(mTime);
266s4.write(memP05[4]);  delay(mTime);
267s1.write(memP06[1]); delay(mTime);
268s2.write(memP06[2]); delay(mTime);
269s3.write(memP06[3]);  delay(mTime);
270s4.write(memP06[4]); delay(mTime);
271s1.write(memP07[1]); delay(mTime);
272s2.write(memP07[2]);  delay(mTime); 
273s3.write(memP07[3]); delay(mTime);
274s4.write(memP07[4]); delay(mTime);
275s1.write(memP08[1]);  delay(mTime);
276s2.write(memP08[2]); delay(mTime); 
277s3.write(memP08[3]); delay(mTime);
278s4.write(memP08[4]);  delay(mTime);
279s1.write(memP09[1]); delay(mTime);
280s2.write(memP09[2]); delay(mTime);
281s3.write(memP09[3]);  delay(mTime);
282s4.write(memP09[4]); delay(mTime);
283s1.write(memP10[1]); delay(mTime);  
284s2.write(memP10[2]); delay(mTime);
285s3.write(memP10[3]); delay(mTime);
286s4.write(memP10[4]);  delay(mTime);
287}
288/*
289  for(int a = 0; a < 5; a++)
290{
291  Serial.println(memP01[a]);
292   Serial.println(memP02[a]);
293    Serial.println(memP03[a]); 
294    Serial.println(memP04[a]);
295     }
296  Serial.println("ln");
297*/
298
299
300void runningScreen() {
301lcd.clear();  
302lcd.setCursor (0,0); lcd.print("PROGRAM      ");
303lcd.setCursor (0,1);  lcd.print("RUNNING >    ");
304delay(100); 
305lcd.setCursor (0,1); lcd.print("RUNNING  >   ");
306delay(100);
307lcd.setCursor (0,1); lcd.print("RUNNING   >  ");
308delay(100);
309lcd.setCursor  (0,1); lcd.print("RUNNING    > ");
310delay(100);
311lcd.setCursor (0,1); lcd.print("RUNNING      ");
312delay(100);
313}
314
315
316void setup() {
317
318// no anoying phisical  resistors to be welded...I hope it works
319pinMode(switchAM, INPUT_PULLUP);
320pinMode(blackButton,  INPUT_PULLUP);
321pinMode(greenButton, INPUT_PULLUP);
322pinMode(redButton, INPUT_PULLUP);
323
324Serial.begin(9600);  // do not need this...the ctrlbox have a lcd.
325lcd.init();
326lcd.backlight();
327  // lcd.begin(16, 2);
328lcd.setCursor(0, 0); lcd.print("ROBOTIC ARM");
329lcd.setCursor(0,  1); lcd.print("VERSION 001");
330delay(3000);
331// attaches the PWM pins to servos
332s1.attach(11);  
333s2.attach(10);
334s3.attach(9);
335s4.attach(6);
336
337} //end setup
338
339void  loop() {
340  
341int switchAMState = digitalRead(switchAM);
342
343  if (switchAMState  == 0){ // Manual Mode
344     lcd.clear();
345     lcd.setCursor(0, 0);
346     lcd.print("MANUAL  OPERATION");
347     
348     valA = analogRead(potpin1);            // reads the  value of the potentiometer (value between 0 and 1023)
349     valA = map(valA, 1,  1023, 180,0);     //values acheived from testing to avoid motor creep at pot end  courses
350     s1.write(valA);
351     delay(15);
352     lcd.setCursor(0, 1);
353     lcd.print("S1: ");
354     lcd.print(valA); //show servo angle on lcd
355
356     valB = analogRead(potpin2);            
357     valB = map(valB, 1, 1023, 180,0);
358     s2.write(valB);
359     delay(15);
360     lcd.setCursor(10, 1);
361     lcd.print("S2:  ");
362     lcd.print(valB);
363
364     valC = analogRead(potpin3);
365     valC  = map(valC, 1, 1023, 0, 180);
366     s3.write(valC);
367     delay(15);
368     lcd.setCursor(0,3);
369     lcd.print("S3: ");
370     lcd.print(valC);
371    
372     valD = analogRead(potpin4);    // reads the value of the potentiometer (value between 0 and 1023)
373     valD  = map(valD, -25, 1023, 0, 180);     // calibrating to avois creeping
374
375//calibration  to let maximum claw opening of 90degrees and pressure excess on claws.dont know  if its useful.
376     if (valD > 40 || valD < 170) { 
377     s4.write(valD);
378     delay(15);    
379     lcd.setCursor(10, 3);
380     lcd.print("S4: ");
381     lcd.print(valD);
382     }
383     else {
384      valD = 41;
385     }
386    
387    delay(100);
388
389int  greenButtonState = digitalRead(greenButton);
390   if (greenButtonState == 0) {
391    counter = counter +1;
392     
393switch  (counter) { 
394  
395case 1:
396memP01[0] = counter; // save counter
397memP01[1]  = valA;
398memP01[2] = valB;
399memP01[3] = valC;
400memP01[4] = valD;
401break;
402
403case  2:
404memP02[0] = counter; // save counter
405memP02[1] = valA; memP02[2] = valB;  memP02[3] = valC; memP02[4] = valD;
406break;
407
408case 3:
409memP03[0] = counter;  // save counter
410memP03[1] = valA; memP03[2] = valB; memP03[3] = valC; memP03[4]  = valD;
411break;
412
413case 4:
414memP04[0] = counter; // save counter
415memP04[1]  = valA; memP04[2] = valB; memP04[3] = valC; memP04[4] = valD; 
416break;
417
418case  5:
419memP05[0] = counter; // save counter
420memP05[1] = valA; memP05[2] = valB;  memP05[3] = valC; memP05[4] = valD;
421break;
422
423case 6:
424memP06[0] = counter;  // save counter
425memP06[1] = valA; memP06[2] = valB; memP06[3] = valC; memP06[4]  = valD;
426break;
427
428case 7:
429memP07[0] = counter; // save counter
430memP07[1]  = valA; memP07[2] = valB; memP07[3] = valC; memP07[4] = valD;
431break;
432
433case  8:
434memP08[0] = counter; // save counter
435memP08[1] = valA; memP08[2] = valB;  memP08[3] = valC; memP08[4] = valD;
436break;
437
438case 9:
439memP09[0] = counter;  // save counter
440memP09[1] = valA; memP09[2] = valB; memP09[3] = valC; memP09[4]  = valD;
441break;
442
443case 10:
444memP10[0] = counter; // save counter
445memP10[1]  = valA; memP10[2] = valB; memP10[3] = valC; memP10[4] = valD;
446break;
447  
448case  11:
449counter =1;
450 
451  
452}//end switch
453     
454lcd.clear();
455lcd.setCursor(0,0);  lcd.print("ARM POSITION ");
456lcd.setCursor(0,1); lcd.print("RECORDED     ");
457lcd.setCursor(0,2);  lcd.print("P  S1  S2  S3  S4");
458lcd.setCursor(0,3); lcd.print(counter);
459lcd.setCursor(4,3);  lcd.print(valA);
460lcd.setCursor(8,3); lcd.print(valB);
461lcd.setCursor(12,3);  lcd.print(valC);
462lcd.setCursor(16,3); lcd.print(valD);
463delay(2000);
464       }  //endgreenbt
465    
466//if needed to correct some  arm position       
467int  blackButtonState = digitalRead(blackButton);
468if (blackButtonState == 0) {
469  if (counter <10){ counter = counter + 1; }
470  else { counter = 1; }
471lcd.clear();
472lcd.setCursor(0,0);  lcd.print("MEM POSITION: ");
473lcd.setCursor(8,1); lcd.print(counter);
474delay(1000);
475        }//end ifblackbt
476         
477    
478    
479 
480     }//end if switchAM
481
482else  { //set ARM to Automatic Mode
483
484if (counter > 0){
485switch (counter){
486
487case  1:
488moveCounter1();
489break;
490case 2:
491moveCounter2();
492break;
493
494case  3:
495moveCounter3();
496break;
497
498case 4:
499moveCounter4();
500break;
501
502case  5:
503moveCounter5();
504break;
505
506case 6:
507moveCounter6();
508break;
509
510case  7:
511moveCounter7();
512break;
513 
514case 8:
515moveCounter8();
516break;
517  
518case 9:
519moveCounter9(); 
520break;
521
522case 10:
523moveCounter10(); 
524break;
525}  //end switch
526  
527  runningScreen();
528 
529} //end if counter>0
530 
531else  { 
532lcd.clear();  
533lcd.setCursor(0,0);  
534lcd.print("EMPTY MEMORY!");
535delay(2000);
536      
537} // end else if counter
538
539} //end else switchAM key
540  
541}   //end  loop

1/*
2 * 6/2/19Versao que funcionou com o control box. apos muitos problemas
3  de mau contato e qualidade inferior
4 * dos servos azuis. Trocados por Towerpro
5  MG90S engrenagem metal.
6 * iniciar montagem do braco MeArm
7 */
8
9#include
10  <Wire.h>
11#include <Servo.h>
12#include <LiquidCrystal_I2C.h> // For 20x4 lcd
13LiquidCrystal_I2C
14  lcd(0x27,20,4);
15
16Servo s1;  // create servo object to control a servo
17Servo
18  s2;
19Servo s3;
20Servo s4;
21
22int potpin1 = A0;  // analog pin used to connect
23  the potentiometer
24int potpin2 = A1;  // analog pin used to connect the potentiometer
25int
26  potpin3 = A2;  // analog pin used to connect the potentiometer
27int potpin4 =
28  A3;  // analog pin used to connect the potentiometer
29
30int valA = 0;    //
31  read the value from the analog pin
32int valB = 0;    // read the value from the
33  analog pin
34int valC = 0;    // read the value from the analog pin
35int valD
36  = 0;    // read the value from the analog pin
37
38int greenButton = 12; //record
39  mem index and servo angles
40int redButton = 4; //reserved for future use
41int
42  blackButton = 7; //mem index selector 
43int switchAM = 8; //switch between Manual
44  or Automatic operation
45int counter = 0; //mem index
46int mTime = 500;//time
47  to move each servo in Aumtomatic mode one by one
48
49//for now this is the way
50  I found to store 10 * 4servo positions.
51//An array of a mem counter and angles
52  of 4 servos
53int memP01[5], memP02[5], memP03[5], memP04[5], memP05[5], memP06[5],
54  memP07[5], memP08[5], memP09[5], memP10[5];
55
56//move ARM according to 4 angles
57  stored in each Array 
58void moveCounter1() { 
59s1.write(memP01[1]); delay(mTime);
60s2.write(memP01[2]);
61  delay(mTime);
62s3.write(memP01[3]); delay(mTime);
63s4.write(memP01[4]); delay(mTime);
64}
65
66void
67  moveCounter2() {
68s1.write(memP01[1]); delay(mTime);
69s2.write(memP01[2]); delay(mTime);
70s3.write(memP01[3]);
71  delay(mTime);
72s4.write(memP01[4]); delay(mTime);
73s1.write(memP02[1]); delay(mTime);
74s2.write(memP02[2]);
75  delay(mTime);
76s3.write(memP02[3]); delay(mTime);
77s4.write(memP02[4]); delay(mTime);
78}
79
80void
81  moveCounter3() {
82s1.write(memP01[1]); delay(mTime);
83s2.write(memP01[2]); delay(mTime);
84s3.write(memP01[3]);
85  delay(mTime);
86s4.write(memP01[4]); delay(mTime);
87s1.write(memP02[1]); delay(mTime);
88s2.write(memP02[2]);
89  delay(mTime);
90s3.write(memP02[3]); delay(mTime);
91s4.write(memP02[4]); delay(mTime);
92s1.write(memP03[1]);
93  delay(mTime); 
94s2.write(memP03[2]); delay(mTime);
95s3.write(memP03[3]); delay(mTime);
96s4.write(memP03[4]);
97  delay(mTime);
98}
99
100void moveCounter4() {
101s1.write(memP01[1]); delay(mTime);
102s2.write(memP01[2]);
103  delay(mTime);
104s3.write(memP01[3]); delay(mTime);
105s4.write(memP01[4]); delay(mTime);
106s1.write(memP02[1]);
107  delay(mTime);
108s2.write(memP02[2]); delay(mTime);
109s3.write(memP02[3]); delay(mTime);
110s4.write(memP02[4]);
111  delay(mTime);
112s1.write(memP03[1]); delay(mTime);
113s2.write(memP03[2]); delay(mTime);
114  
115s3.write(memP03[3]); delay(mTime);
116s4.write(memP03[4]); delay(mTime);
117s1.write(memP04[1]);
118  delay(mTime);
119s2.write(memP04[2]); delay(mTime);
120s3.write(memP04[3]); delay(mTime);
121s4.write(memP04[4]);
122  delay(mTime);
123}
124
125void moveCounter5() {
126s1.write(memP01[1]); delay(mTime);
127s2.write(memP01[2]);
128  delay(mTime); 
129s3.write(memP01[3]); delay(mTime);
130s4.write(memP01[4]); delay(mTime);
131s1.write(memP02[1]);
132  delay(mTime); 
133s2.write(memP02[2]); delay(mTime); 
134s3.write(memP02[3]); delay(mTime);
135s4.write(memP02[4]);
136  delay(mTime);
137s1.write(memP03[1]); delay(mTime);
138s2.write(memP03[2]); delay(mTime);
139  
140s3.write(memP03[3]); delay(mTime);
141s4.write(memP03[4]); delay(mTime);
142s1.write(memP04[1]);
143  delay(mTime); 
144s2.write(memP04[2]); delay(mTime);
145s3.write(memP04[3]); delay(mTime);
146s4.write(memP04[4]);
147  delay(mTime);
148s1.write(memP05[1]); delay(mTime); 
149s2.write(memP05[2]); delay(mTime);
150s3.write(memP05[3]);
151  delay(mTime);
152s4.write(memP05[4]); delay(mTime);
153}
154
155void moveCounter6()
156  {
157s1.write(memP01[1]); delay(mTime);
158s2.write(memP01[2]); delay(mTime);
159s3.write(memP01[3]);
160  delay(mTime);
161s4.write(memP01[4]); delay(mTime);
162s1.write(memP02[1]); delay(mTime);
163s2.write(memP02[2]);
164  delay(mTime);
165s3.write(memP02[3]); delay(mTime);
166s4.write(memP02[4]); delay(mTime);
167s1.write(memP03[1]);
168  delay(mTime);
169s2.write(memP03[2]); delay(mTime);
170s3.write(memP03[3]); delay(mTime);
171s4.write(memP03[4]);
172  delay(mTime);
173s1.write(memP04[1]); delay(mTime); 
174s2.write(memP04[2]); delay(mTime);
175s3.write(memP04[3]);
176  delay(mTime);
177s4.write(memP04[4]); delay(mTime);
178s1.write(memP05[1]); delay(mTime);
179s2.write(memP05[2]);
180  delay(mTime); 
181s3.write(memP05[3]); delay(mTime);
182s4.write(memP05[4]); delay(mTime);
183s1.write(memP06[1]);
184  delay(mTime); 
185s2.write(memP06[2]); delay(mTime); 
186s3.write(memP06[3]); delay(mTime);
187s4.write(memP06[4]);
188  delay(mTime);
189}
190
191void moveCounter7() {
192s1.write(memP01[1]); delay(mTime);
193s2.write(memP01[2]);
194  delay(mTime);
195s3.write(memP01[3]); delay(mTime);
196s4.write(memP01[4]); delay(mTime);
197s1.write(memP02[1]);
198  delay(mTime);
199s2.write(memP02[2]); delay(mTime);
200s3.write(memP02[3]); delay(mTime);
201s4.write(memP02[4]);
202  delay(mTime);
203s1.write(memP03[1]); delay(mTime);
204s2.write(memP03[2]); delay(mTime);
205  
206s3.write(memP03[3]); delay(mTime);
207s4.write(memP03[4]); delay(mTime);
208s1.write(memP04[1]);
209  delay(mTime);
210s2.write(memP04[2]); delay(mTime); 
211s3.write(memP04[3]); delay(mTime);
212s4.write(memP04[4]);
213  delay(mTime);
214s1.write(memP05[1]); delay(mTime);
215s2.write(memP05[2]); delay(mTime);
216  
217s3.write(memP05[3]); delay(mTime);
218s4.write(memP05[4]); delay(mTime);
219s1.write(memP06[1]);
220  delay(mTime); 
221s2.write(memP06[2]); delay(mTime);
222s3.write(memP06[3]); delay(mTime);
223s4.write(memP06[4]);
224  delay(mTime);
225s1.write(memP07[1]); delay(mTime);
226s2.write(memP07[2]); delay(mTime);
227s3.write(memP07[3]);
228  delay(mTime);
229s4.write(memP07[4]); delay(mTime);
230}
231
232void moveCounter8()
233  {
234s1.write(memP01[1]); delay(mTime);
235s2.write(memP01[2]); delay(mTime);
236s3.write(memP01[3]);
237  delay(mTime);
238s4.write(memP01[4]); delay(mTime);
239s1.write(memP02[1]); delay(mTime);
240  
241s2.write(memP02[2]); delay(mTime);
242s3.write(memP02[3]); delay(mTime);
243s4.write(memP02[4]);
244  delay(mTime);
245s1.write(memP03[1]); delay(mTime);
246s2.write(memP03[2]); delay(mTime);
247s3.write(memP03[3]);
248  delay(mTime);
249s4.write(memP03[4]); delay(mTime);
250s1.write(memP04[1]); delay(mTime);
251  
252s2.write(memP04[2]); delay(mTime); 
253s3.write(memP04[3]); delay(mTime);
254s4.write(memP04[4]);
255  delay(mTime);
256s1.write(memP05[1]); delay(mTime);
257s2.write(memP05[2]); delay(mTime);
258  
259s3.write(memP05[3]); delay(mTime);
260s4.write(memP05[4]); delay(mTime);
261s1.write(memP06[1]);
262  delay(mTime); 
263s2.write(memP06[2]); delay(mTime); 
264s3.write(memP06[3]); delay(mTime);
265s4.write(memP06[4]);
266  delay(mTime);
267s1.write(memP07[1]); delay(mTime);
268s2.write(memP07[2]); delay(mTime);
269  
270s3.write(memP07[3]); delay(mTime);
271s4.write(memP07[4]); delay(mTime);
272s1.write(memP08[1]);
273  delay(mTime);
274s2.write(memP08[2]); delay(mTime); 
275s3.write(memP08[3]); delay(mTime);
276s4.write(memP08[4]);
277  delay(mTime);
278}
279
280void moveCounter9() {
281s1.write(memP01[1]); delay(mTime);
282s2.write(memP01[2]);
283  delay(mTime);
284s3.write(memP01[3]); delay(mTime); 
285s4.write(memP01[4]); delay(mTime);
286s1.write(memP02[1]);
287  delay(mTime);
288s2.write(memP02[2]); delay(mTime); 
289s3.write(memP02[3]); delay(mTime);
290  
291s4.write(memP02[4]); delay(mTime);
292s1.write(memP03[1]); delay(mTime);
293s2.write(memP03[2]);
294  delay(mTime); 
295s3.write(memP03[3]); delay(mTime); 
296s4.write(memP03[4]); delay(mTime);
297s1.write(memP04[1]);
298  delay(mTime);
299s2.write(memP04[2]); delay(mTime); 
300s3.write(memP04[3]); delay(mTime);
301s4.write(memP04[4]);
302  delay(mTime);
303s1.write(memP05[1]); delay(mTime);
304s2.write(memP05[2]); delay(mTime);
305  
306s3.write(memP05[3]); delay(mTime);
307s4.write(memP05[4]); delay(mTime);
308s1.write(memP06[1]);
309  delay(mTime); 
310s2.write(memP06[2]); delay(mTime);
311s3.write(memP06[3]); delay(mTime);
312  
313s4.write(memP06[4]); delay(mTime);
314s1.write(memP07[1]); delay(mTime);
315s2.write(memP07[2]);
316  delay(mTime);
317s3.write(memP07[3]); delay(mTime); 
318s4.write(memP07[4]); delay(mTime);
319s1.write(memP08[1]);
320  delay(mTime);
321s2.write(memP08[2]); delay(mTime);
322s3.write(memP08[3]); delay(mTime);
323  
324s4.write(memP08[4]); delay(mTime);
325s1.write(memP09[1]); delay(mTime); 
326s2.write(memP09[2]);
327  delay(mTime); 
328s3.write(memP09[3]); delay(mTime);
329s4.write(memP09[4]); delay(mTime);
330}
331
332void
333  moveCounter10() {
334s1.write(memP01[1]); delay(mTime); 
335s2.write(memP01[2]);
336  delay(mTime);
337s3.write(memP01[3]); delay(mTime);
338s4.write(memP01[4]); delay(mTime);
339s1.write(memP02[1]);
340  delay(mTime);
341s2.write(memP02[2]); delay(mTime);
342s3.write(memP02[3]); delay(mTime);
343s4.write(memP02[4]);
344  delay(mTime);
345s1.write(memP03[1]); delay(mTime);
346s2.write(memP03[2]); delay(mTime);
347s3.write(memP03[3]);
348  delay(mTime);
349s4.write(memP03[4]); delay(mTime);
350s1.write(memP04[1]); delay(mTime);
351s2.write(memP04[2]);
352  delay(mTime); 
353s3.write(memP04[3]); delay(mTime);
354s4.write(memP04[4]); delay(mTime);
355s1.write(memP05[1]);
356  delay(mTime);
357s2.write(memP05[2]); delay(mTime);
358s3.write(memP05[3]); delay(mTime);
359s4.write(memP05[4]);
360  delay(mTime);
361s1.write(memP06[1]); delay(mTime);
362s2.write(memP06[2]); delay(mTime);
363s3.write(memP06[3]);
364  delay(mTime);
365s4.write(memP06[4]); delay(mTime);
366s1.write(memP07[1]); delay(mTime);
367s2.write(memP07[2]);
368  delay(mTime); 
369s3.write(memP07[3]); delay(mTime);
370s4.write(memP07[4]); delay(mTime);
371s1.write(memP08[1]);
372  delay(mTime);
373s2.write(memP08[2]); delay(mTime); 
374s3.write(memP08[3]); delay(mTime);
375s4.write(memP08[4]);
376  delay(mTime);
377s1.write(memP09[1]); delay(mTime);
378s2.write(memP09[2]); delay(mTime);
379s3.write(memP09[3]);
380  delay(mTime);
381s4.write(memP09[4]); delay(mTime);
382s1.write(memP10[1]); delay(mTime);
383  
384s2.write(memP10[2]); delay(mTime);
385s3.write(memP10[3]); delay(mTime);
386s4.write(memP10[4]);
387  delay(mTime);
388}
389/*
390  for(int a = 0; a < 5; a++)
391{
392  Serial.println(memP01[a]);
393
394   Serial.println(memP02[a]);
395    Serial.println(memP03[a]); 
396    Serial.println(memP04[a]);
397
398     }
399  Serial.println("ln");
400*/
401
402
403void runningScreen() {
404lcd.clear();
405  
406lcd.setCursor (0,0); lcd.print("PROGRAM      ");
407lcd.setCursor (0,1);
408  lcd.print("RUNNING >    ");
409delay(100); 
410lcd.setCursor (0,1); lcd.print("RUNNING
411  >   ");
412delay(100);
413lcd.setCursor (0,1); lcd.print("RUNNING   >  ");
414delay(100);
415lcd.setCursor
416  (0,1); lcd.print("RUNNING    > ");
417delay(100);
418lcd.setCursor (0,1); lcd.print("RUNNING
419      ");
420delay(100);
421}
422
423
424void setup() {
425
426// no anoying phisical
427  resistors to be welded...I hope it works
428pinMode(switchAM, INPUT_PULLUP);
429pinMode(blackButton,
430  INPUT_PULLUP);
431pinMode(greenButton, INPUT_PULLUP);
432pinMode(redButton, INPUT_PULLUP);
433
434Serial.begin(9600);
435  // do not need this...the ctrlbox have a lcd.
436lcd.init();
437lcd.backlight();
438
439  // lcd.begin(16, 2);
440lcd.setCursor(0, 0); lcd.print("ROBOTIC ARM");
441lcd.setCursor(0,
442  1); lcd.print("VERSION 001");
443delay(3000);
444// attaches the PWM pins to servos
445s1.attach(11);
446  
447s2.attach(10);
448s3.attach(9);
449s4.attach(6);
450
451} //end setup
452
453void
454  loop() {
455  
456int switchAMState = digitalRead(switchAM);
457
458  if (switchAMState
459  == 0){ // Manual Mode
460     lcd.clear();
461     lcd.setCursor(0, 0);
462     lcd.print("MANUAL
463  OPERATION");
464     
465     valA = analogRead(potpin1);            // reads the
466  value of the potentiometer (value between 0 and 1023)
467     valA = map(valA, 1,
468  1023, 180,0);     //values acheived from testing to avoid motor creep at pot end
469  courses
470     s1.write(valA);
471     delay(15);
472     lcd.setCursor(0, 1);
473
474     lcd.print("S1: ");
475     lcd.print(valA); //show servo angle on lcd
476
477
478     valB = analogRead(potpin2);            
479     valB = map(valB, 1, 1023, 180,0);
480
481     s2.write(valB);
482     delay(15);
483     lcd.setCursor(10, 1);
484     lcd.print("S2:
485  ");
486     lcd.print(valB);
487
488     valC = analogRead(potpin3);
489     valC
490  = map(valC, 1, 1023, 0, 180);
491     s3.write(valC);
492     delay(15);
493     lcd.setCursor(0,3);
494
495     lcd.print("S3: ");
496     lcd.print(valC);
497    
498     valD = analogRead(potpin4);
499    // reads the value of the potentiometer (value between 0 and 1023)
500     valD
501  = map(valD, -25, 1023, 0, 180);     // calibrating to avois creeping
502
503//calibration
504  to let maximum claw opening of 90degrees and pressure excess on claws.dont know
505  if its useful.
506     if (valD > 40 || valD < 170) { 
507     s4.write(valD);
508
509     delay(15);    
510     lcd.setCursor(10, 3);
511     lcd.print("S4: ");
512
513     lcd.print(valD);
514     }
515     else {
516      valD = 41;
517     }
518
519    
520    delay(100);
521
522int  greenButtonState = digitalRead(greenButton);
523
524   if (greenButtonState == 0) {
525    counter = counter +1;
526     
527switch
528  (counter) { 
529  
530case 1:
531memP01[0] = counter; // save counter
532memP01[1]
533  = valA;
534memP01[2] = valB;
535memP01[3] = valC;
536memP01[4] = valD;
537break;
538
539case
540  2:
541memP02[0] = counter; // save counter
542memP02[1] = valA; memP02[2] = valB;
543  memP02[3] = valC; memP02[4] = valD;
544break;
545
546case 3:
547memP03[0] = counter;
548  // save counter
549memP03[1] = valA; memP03[2] = valB; memP03[3] = valC; memP03[4]
550  = valD;
551break;
552
553case 4:
554memP04[0] = counter; // save counter
555memP04[1]
556  = valA; memP04[2] = valB; memP04[3] = valC; memP04[4] = valD; 
557break;
558
559case
560  5:
561memP05[0] = counter; // save counter
562memP05[1] = valA; memP05[2] = valB;
563  memP05[3] = valC; memP05[4] = valD;
564break;
565
566case 6:
567memP06[0] = counter;
568  // save counter
569memP06[1] = valA; memP06[2] = valB; memP06[3] = valC; memP06[4]
570  = valD;
571break;
572
573case 7:
574memP07[0] = counter; // save counter
575memP07[1]
576  = valA; memP07[2] = valB; memP07[3] = valC; memP07[4] = valD;
577break;
578
579case
580  8:
581memP08[0] = counter; // save counter
582memP08[1] = valA; memP08[2] = valB;
583  memP08[3] = valC; memP08[4] = valD;
584break;
585
586case 9:
587memP09[0] = counter;
588  // save counter
589memP09[1] = valA; memP09[2] = valB; memP09[3] = valC; memP09[4]
590  = valD;
591break;
592
593case 10:
594memP10[0] = counter; // save counter
595memP10[1]
596  = valA; memP10[2] = valB; memP10[3] = valC; memP10[4] = valD;
597break;
598  
599case
600  11:
601counter =1;
602 
603  
604}//end switch
605     
606lcd.clear();
607lcd.setCursor(0,0);
608  lcd.print("ARM POSITION ");
609lcd.setCursor(0,1); lcd.print("RECORDED     ");
610lcd.setCursor(0,2);
611  lcd.print("P  S1  S2  S3  S4");
612lcd.setCursor(0,3); lcd.print(counter);
613lcd.setCursor(4,3);
614  lcd.print(valA);
615lcd.setCursor(8,3); lcd.print(valB);
616lcd.setCursor(12,3);
617  lcd.print(valC);
618lcd.setCursor(16,3); lcd.print(valD);
619delay(2000);
620       }
621  //endgreenbt
622    
623//if needed to correct some  arm position       
624int
625  blackButtonState = digitalRead(blackButton);
626if (blackButtonState == 0) {
627
628  if (counter <10){ counter = counter + 1; }
629  else { counter = 1; }
630lcd.clear();
631lcd.setCursor(0,0);
632  lcd.print("MEM POSITION: ");
633lcd.setCursor(8,1); lcd.print(counter);
634delay(1000);
635
636        }//end ifblackbt
637         
638    
639    
640 
641     }//end if switchAM
642
643else
644  { //set ARM to Automatic Mode
645
646if (counter > 0){
647switch (counter){
648
649case
650  1:
651moveCounter1();
652break;
653case 2:
654moveCounter2();
655break;
656
657case
658  3:
659moveCounter3();
660break;
661
662case 4:
663moveCounter4();
664break;
665
666case
667  5:
668moveCounter5();
669break;
670
671case 6:
672moveCounter6();
673break;
674
675case
676  7:
677moveCounter7();
678break;
679 
680case 8:
681moveCounter8();
682break;
683
684  
685case 9:
686moveCounter9(); 
687break;
688
689case 10:
690moveCounter10(); 
691break;
692}
693  //end switch
694  
695  runningScreen();
696 
697} //end if counter>0
698 
699else
700  { 
701lcd.clear();  
702lcd.setCursor(0,0);  
703lcd.print("EMPTY MEMORY!");
704delay(2000);
705
706      
707} // end else if counter
708
709} //end else switchAM key
710  
711}   //end
712  loop