KjellFa Robot: A Replica Of The Trallfa Robot

1//Libraies
2#include <SPI.h>
3#include <SdFat.h>
4#include <IRremote.h>
5#include  <Servo.h>
6SdFat SD;  
7
8
9//Remote buttons
10int play = 0; // = #
11int  nextStep = 0; // Arrow up
12int Stop = 0; // = Star
13int Rec = 0; //= OK
14int  writeSD=0; //Arrow down
15int delAll=0;  //Arrow left
16
17int posReached =0;    //All servos have reache their positions
18//int stepCount = -1;
19int val  = HIGH;       //Memory for Repeat action (Low if Repeat)
20int servo1, servo2,  servo3, servo4, servo5, servo6; 
21//int stepVal;
22//int recVal;
23int velo;
24
25
26//Input/  outputs
27int RECV_PIN = 15; //Pin select IR
28int CSPin =10;    //Chipselect  SD
29int LED=16;       //LED in front
30
31
32//IR
33IRrecv irrecv(RECV_PIN);
34decode_results  results;
35
36const int activeServos =6;    //No of Servos in action
37
38const  int top=15;     //Max no of storage in array
39
40unsigned long codeValue;  //Detection  by IR
41
42//define servo objects
43Servo S1, S2, S3, S4, S5, S6;  
44
45int  armIs[activeServos] ;        // array "is" Position
46int armPos[top][activeServos];    // array to hold arm positions up to defined by "top"
47int posCount = 0;                 // to count number of positions increased when "save" button  pressed
48int posCountMax;                  // max number of positions recorded  
49
50
51File Robot;                       //Name of file for SD
52
53
54//-------------------------------------------------------------------OK
55void  setup() 
56{
57const int homeS1 =90 , homeS2 = 94, homeS3 = 170, homeS4 = 150,  homeS5 = 68, homeS6 = 70;  //Home position
58 pinMode(LED, OUTPUT);
59  Serial.begin(9600);
60  irrecv.enableIRIn(); // Start the receiver
61  
62  // attach servos to relevent  pins on arduino nano
63  S1.attach(2);
64  S2.attach(3);
65  S3.attach(4);
66  S4.attach(5);
67  S5.attach(6);
68  S6.attach(7);
69 
70
71  //Home position  when start up.
72  S1.write(homeS1);
73  S2.write(homeS2);
74  S3.write(homeS3);
75  S4.write(homeS4);
76  S5.write(homeS5);
77  S6.write(homeS6);
78
79//SetActual  Position
80  armIs[0]=homeS1;
81  armIs[1]=homeS2;
82  armIs[2]=homeS3;
83  armIs[3]=homeS4;  
84  armIs[4]=homeS5; 
85  armIs[5]=homeS6;
86
87 Serial.print("Initializing  SD card...");
88
89      if (!SD.begin(CSPin)) 
90      {
91       Serial.println("initialization  failed!");
92      return;
93      }
94      Serial.println("initialization  done.");
95
96//readSD vaules at start-up into array
97     
98      Robot  = SD.open("prgm.txt");
99        if (Robot) 
100        {
101        Serial.println("prgm.txt  is loaded from SD");
102              // read from the file until there's nothing  else in it:
103              while (Robot.available()) 
104              {
105//                  Serial.println(Robot.read());              
106                for  (int i =0; i<activeServos; i++)
107                {
108                  armPos[posCount][i]  = (Robot.read());
109                }
110               posCount= posCount+1;
111              }
112             posCountMax=posCount;       
113 //            Serial.print("posCountMax  = "); Serial.println(posCountMax-1);
114             // close the file:
115            Robot.close();
116        }
117        else
118        {
119        // if the file didn't open, print  an error:
120        Serial.println("error opening prgm.txt or file not present");
121        }
122
123 Serial.println("KjellFa is ready for action");
124int t2= 30;
125  digitalWrite(LED, LOW);    //Gives a "long" LED signal when action is acknowledged
126  delay(t2*40);
127  digitalWrite(LED, HIGH);
128
129} //End SetUp
130
131
132//----------------Loop---------------------------------------------------OK
133
134void  loop()
135{
136  readIR();       //done
137  codeDecode();   //done
138  Record();       //done
139  playBack();     //done
140  stopMove();     //done
141  movebyIR();     //done
142  writetoSD();    //done
143  deleteAll();    //done
144
145
146}
147
148//-------------------------------------------------------------OK
149void  readIR()
150{
151  if (irrecv.decode(&results))
152  {
153    codeValue = (results.value);
154   Serial.print("codeValue = "); Serial.println(codeValue); 
155    delay(100);    
156    irrecv.resume(); // Receive the next value
157
158  }
159}
160
161//-------------------------------------------------------------------OK
162void   codeDecode()
163{
164    if (codeValue == 16738455) {servo1 = 12;} //"1" Servo1  Left
165    if (codeValue == 16750695) {servo1 = 11;} //"2" Servo1 Right
166    if  (codeValue == 16724175) {servo2 = 22;} //"4" Servo2 Forward
167    if (codeValue  == 16718055) {servo2 = 21;} //"5" Servo2 Backwards
168    if (codeValue == 16756815)  {servo3 = 32;} //"3" Servo3 Up
169    if (codeValue == 16743045) {servo3 = 31;}  //"6" Servo3 Down
170    if (codeValue == 16734885) {servo4 = 42;} //"9" Servo4  up
171    if (codeValue == 16730805) {servo4 = 41;} //"0" Servo4down
172    if  (codeValue == 16716015) {servo5 = 51;} //"7" Servo5Left
173    if (codeValue ==  16726215) {servo5 = 52;} //"8" Servorighr
174    if (codeValue == 16720605) {servo6  = 61;} //"<" Servo6 Open / Close
175    if (codeValue == 16761405) {delAll = 1;}  //">" Delete all both SD and RAM
176    if (codeValue == 16754775) {writeSD =  1;} //"V" WritetoCD
177    if (codeValue == 16736925) {nextStep = 1;} //"A"  Next step
178    if (codeValue == 16732845) {play = 1;} //"#" Play
179    if (codeValue  == 16712445) {Rec = 1;} //OK  position stored in arrary no
180    if (codeValue  == 16728765) {Stop = 1;} //"*" Stop
181}
182     
183
184//-------------------------------------------------------------------OK
185void  Record()
186{
187  if (Rec == 1)
188  {
189  if(posCount<top-1)
190  {
191        for  (int i =0; i<activeServos; i ++)
192         {
193         armPos[posCount][i] =  armIs[i];
194         }
195        posCount = posCount + 1; posCountMax = posCount;
196
197
198      stopRec();            //Stop moving servos when ""Rec" button is pressed
199    } 
200    blinkUp();
201  }
202  codeValue = 0;  //Reset input selection
203  Rec = 0;        //Reset routine
204}
205
206
207//-----------------------------------------------------------------------
208
209void  playBack()
210{
211  int val =1;   //Robot will repeat program until velo speed  is below setting and val=0
212  if (play == 1)
213  {
214   posCount =0;
215
216  blinkUp();
217
218    while(posCount<posCountMax)
219    {
220      veloSpeed();
221      Read(); 
222      playServo();   // physically move arm to updated position,  this is broken into small steps
223     delay(velo);   // pause between moves 
224
225    readIR();
226   if (codeValue == 16728765) {blinkUp(); posCount= posCountMax;  val=LOW; codeValue =0; play =0;} 
227      if(velo > 40)
228      {
229      val=LOW;
230      } 
231      if ((posCount == posCountMax) && (val == HIGH)) 
232      {
233       posCount = 0; 
234       }
235
236    }
237  }
238  codeValue = 0;  //Reset  input selection
239  play = 0;       //Reset routine
240}
241
242
243
244//----------------------------------------------------------------------OK
245void  writetoSD()
246{
247    if(writeSD==1)
248    {
249  blinkUp();
250      
251      SD.remove("prgm.txt");
252     Serial.println("Old prgm.txt is deleted from SD");
253      posCount = 0;
254      Robot = SD.open("prgm.txt", FILE_WRITE);
255
256
257        if (Robot) 
258        {
259
260               Serial.println("New prgm is written to SD :");             
261            while(posCount< posCountMax)
262            {
263              for(int  i=0; i<activeServos; i++)
264              {
265              Robot.write(armPos[posCount][i]);
266
267  //            Serial.print("Step =");Serial.print(posCount); Serial.print("  Servo  ");Serial.print(i); Serial.print("   Value = ");
268 //            Serial.println(armPos[posCount][i]);
269              }
270              posCount ++;   
271                 
272            }   
273
274        // close the file:
275        Robot.close();
276        Serial.println("done.");
277        } 
278        else 
279       {
280        // if the file didn't open, print  an error:
281        Serial.println("error writing to prgm.txt");
282       }
283
284
285    }
286codeValue =0;
287writeSD = 0;
288}
289//--------------------------------------------------------------------------------------OK
290void  deleteAll()
291  {
292    if ( delAll == 1)  //Del button arraw down
293  {
294blinkUp();
295      
296      SD.remove("prgm.txt");
297//      Serial.println("prgm.txt is  deleted from SD");
298
299      while(posCount<posCountMax)
300      {
301        for  (int i=0; i<activeServos; i++)
302        {
303        armPos[posCount][i] = {00};
304        }
305        posCount ++;
306      }
307//      Serial.println("Flash memo  is cleared and deleted");
308      posCount = 0;
309      posCountMax = 0;
310  }
311    codeValue = 0;      //Reset input selection
312    delAll=0;           //Reset  routine
313  }
314
315
316
317//----------------------------------------------------------------OK
318void  stopMove()
319{
320  if (Stop == 1)  //Stop button
321  {
322
323blinkUp();
324
325    servo1 = servo2 = servo3 = servo4 = servo5= servo6 = 0;
326
327
328    codeValue  = 0;    //Reset input selection
329    Stop = 0;         //reset routine
330  }
331}
332
333
334
335//----------------------------------------------------------------------------OK
336
337void  movebyIR()
338{
339const int S1min = 18, S1max = 166, S2min = 20, S2max = 175, S3min  = 60, S3max = 177, 
340          S4min = 5, S4max = 172, S5min = 2, S5max = 180,  S6min = 70, S6max = 90;
341const int v = 1 ;   //Increments in for loops movebyIR
342          
343veloSpeed();
344
345
346  //Servo1
347  if ((servo1 == 11) && (armIs[0]  > S1min))
348  {
349 blinkUp();
350
351// Serial.print("Servo1 = "); Serial.println  (armIs[0]);
352
353    armIs[0] = armIs[0] - v;
354    S1.write(armIs[0]);
355    delay(velo);
356  }
357
358  if ((servo1 == 12) && (armIs[0] < S1max))
359    {
360blinkUp();
361
362//Serial.print("Servo1  = "); Serial.println (armIs[0]);
363
364    armIs[0] = armIs[0] + v;
365    S1.write(armIs[0]);
366    delay(velo);
367  }
368
369  //Servo2
370  if ((servo2 == 21) && (armIs[1] >  S2min))
371  { 
372blinkUp();
373//Serial.print("Servo2  = "); Serial.println  (armIs[1]);
374    armIs[1] = armIs[1] - v;
375    S2.write(armIs[1]);
376    delay(velo);
377  }
378
379  if ((servo2 == 22) && (armIs[1] < S2max))
380  {
381blinkUp();
382   
383//Serial.print("Servo2  = "); Serial.println (armIs[1]);
384    armIs[1]  = armIs[1] + v;
385    S2.write(armIs[1]);
386    delay(velo);
387  }
388
389  //Servo3
390  if ((servo3 == 31) && (armIs[2] > S3min))
391  { 
392blinkUp();
393//Serial.print("Servo3  = "); Serial.println (armIs[2]);
394    armIs[2] = armIs[2] - v;
395    S3.write(armIs[2]);
396    delay(velo);
397  }
398
399  if ((servo3 == 32) && (armIs[2] < S3max))
400  {      
401 blinkUp();
402//Serial.print("Servo3  = "); Serial.println (armIs[2]);
403    armIs[2] = armIs[2] + v;
404    S3.write(armIs[2]);
405    delay(velo);
406  }
407
408  //Servo 4
409  if ((servo4 == 41) && (armIs[3] > S4min))
410  {      
411  blinkUp();
412 //Serial.print("Servo4  = "); Serial.println (armIs[3]);
413    armIs[3]  = armIs[3] - v;
414    S4.write(armIs[3]);
415    delay(velo);
416  }
417
418  if  ((servo4 == 42) && (armIs[3] < S4max))
419  {      
420blinkUp();
421 //Serial.print("Servo4  = "); Serial.println (armIs[3]);
422    armIs[3] = armIs[3] + v;
423    S4.write(armIs[3]);
424    delay(velo);
425  }
426
427  //Servo 5
428  if ((servo5 == 51) && (armIs[4]  > S5min))
429  {      
430blinkUp();
431// Serial.print("Servo5  = "); Serial.println  (armIs[4]);
432    armIs[4] = armIs[4] - v;
433    S5.write(armIs[4]);
434    delay(velo);
435  }
436
437  if ((servo5 == 52) && (armIs[4] < S5max))
438  {      
439blinkUp();
440  //Serial.print("Servo5  = "); Serial.println (armIs[4]);
441    armIs[4] = armIs[4]  + v;
442    S5.write(armIs[4]);
443    delay(velo);
444  }
445
446 
447  //Servo  6
448  if ((servo6 == 61) && (armIs[5] <= S6min))
449  {      
450blinkUp();
451
452      while(armIs[5] <= S6max)
453      {
454 //    Serial.print("Servo6  = ");  Serial.println (armIs[5]);
455      armIs[5]++;
456      S6.write(armIs[5]);
457      delay(velo);
458      }
459     codeValue = 0;
460  servo6 =0;   
461  }
462
463  if ((servo6 == 61) && (armIs[5] >= S6max))
464  {      
465blinkUp();
466  
467      while(armIs[5] >=S6min)
468      {
469 //   Serial.print("Servo6  = ");  Serial.println (armIs[5]);
470      armIs[5] --;
471      S6.write(armIs[5]);
472      delay(velo);
473      }
474  codeValue = 0;
475  servo6 =0;
476  }
477
478}
479
480//--------------------Sub  Routine for veloSpeed------------------------------------
481void veloSpeed()
482{
483
484  velo = analogRead(0);                   //Reading potentiometer value from pin  A0
485  velo = map(velo, 1024, 0, 50, 1);      //Map the value to a good velocity  value - velo
486
487} 
488
489//-----------Sub Routine to stop move when OK is pressed  for direct recording----------OK
490void stopRec()
491{
492blinkUp();
493
494    servo1  = servo2 = servo3 = servo4 = servo5 = servo6 = 0;
495}
496
497//-------------------Sub  Routine to read and move from point to point----------OK
498void Read()
499{
500int  posReached;
501  posReached = 0;
502  for (int i = 0; i < activeServos ; i++ ) 
503  {
504     if ( armIs[i] > armPos[posCount][i]) armIs[i] = armIs[i] - 1; 
505    if  ( armIs[i] < armPos[posCount][i]) armIs[i] = armIs[i] + 1; 
506    if ( armIs[i]  == armPos[posCount][i]) posReached++; 
507  }
508    if (posReached == activeServos)  posCount ++;  
509}
510
511//-----------------Sub Routine to move Servos to new  position-------------OK
512void playServo()
513{  //Move all servos to new position
514  S1.write(armIs[0]);
515  S2.write(armIs[1]);
516  S3.write(armIs[2]);
517  S4.write(armIs[3]);
518  S5.write(armIs[4]);
519  S6.write(armIs[5]);
520}
521
522
523//----Sub Routine  to give a flash when a button is operated--------OK
524void blinkUp()
525{
526const  int t2 =80;   //Length of LED Blink
527  digitalWrite(LED, LOW);   
528  delay(t2);                //Length of flash
529  digitalWrite(LED, HIGH ); 
530}
531
532
533