kjaeger
Published © GPL3+

KjellFa Robot: A Replica Of The Trallfa Robot

A 5-axis robot, all 3D-printed, with an Arduino Nano and miniSD card. Path programming and control by IR remote.

IntermediateShowcase (no instructions)838
KjellFa Robot: A Replica Of The Trallfa Robot

Things used in this project

Hardware components

Arduino Nano R3
Arduino Nano R3
×1
MicroSD Card with Adapter
Digilent MicroSD Card with Adapter
×1
SG90
×6
CPU card
×1
PSU 110/220VAC to 5VDC
×1

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)

Story

Read more

Custom parts and enclosures

KjellFa Robot

Code

Robot_ver_2

Arduino
//Libraies
#include <SPI.h>
#include <SdFat.h>
#include <IRremote.h>
#include <Servo.h>
SdFat SD;  


//Remote buttons
int play = 0; // = #
int nextStep = 0; // Arrow up
int Stop = 0; // = Star
int Rec = 0; //= OK
int writeSD=0; //Arrow down
int delAll=0;  //Arrow left

int posReached =0;    //All servos have reache their positions
//int stepCount = -1;
int val = HIGH;       //Memory for Repeat action (Low if Repeat)
int servo1, servo2, servo3, servo4, servo5, servo6; 
//int stepVal;
//int recVal;
int velo;


//Input/ outputs
int RECV_PIN = 15; //Pin select IR
int CSPin =10;    //Chipselect SD
int LED=16;       //LED in front


//IR
IRrecv irrecv(RECV_PIN);
decode_results results;

const int activeServos =6;    //No of Servos in action

const int top=15;     //Max no of storage in array

unsigned long codeValue;  //Detection by IR

//define servo objects
Servo S1, S2, S3, S4, S5, S6;  

int armIs[activeServos] ;        // array "is" Position
int armPos[top][activeServos];    // array to hold arm positions up to defined by "top"
int posCount = 0;                 // to count number of positions increased when "save" button pressed
int posCountMax;                  // max number of positions recorded 


File Robot;                       //Name of file for SD


//-------------------------------------------------------------------OK
void setup() 
{
const int homeS1 =90 , homeS2 = 94, homeS3 = 170, homeS4 = 150, homeS5 = 68, homeS6 = 70;  //Home position
 pinMode(LED, OUTPUT);
  Serial.begin(9600);
  irrecv.enableIRIn(); // Start the receiver
  
  // attach servos to relevent pins on arduino nano
  S1.attach(2);
  S2.attach(3);
  S3.attach(4);
  S4.attach(5);
  S5.attach(6);
  S6.attach(7);
 

  //Home position when start up.
  S1.write(homeS1);
  S2.write(homeS2);
  S3.write(homeS3);
  S4.write(homeS4);
  S5.write(homeS5);
  S6.write(homeS6);

//SetActual Position
  armIs[0]=homeS1;
  armIs[1]=homeS2;
  armIs[2]=homeS3;
  armIs[3]=homeS4; 
  armIs[4]=homeS5; 
  armIs[5]=homeS6;

 Serial.print("Initializing SD card...");

      if (!SD.begin(CSPin)) 
      {
       Serial.println("initialization failed!");
      return;
      }
      Serial.println("initialization done.");

//readSD vaules at start-up into array
     
      Robot = SD.open("prgm.txt");
        if (Robot) 
        {
        Serial.println("prgm.txt is loaded from SD");
              // read from the file until there's nothing else in it:
              while (Robot.available()) 
              {
//                  Serial.println(Robot.read());              
                for (int i =0; i<activeServos; i++)
                {
                  armPos[posCount][i] = (Robot.read());
                }
               posCount= posCount+1;
              }
             posCountMax=posCount;       
 //            Serial.print("posCountMax = "); Serial.println(posCountMax-1);
             // close the file:
            Robot.close();
        }
        else
        {
        // if the file didn't open, print an error:
        Serial.println("error opening prgm.txt or file not present");
        }

 Serial.println("KjellFa is ready for action");
int t2= 30;
  digitalWrite(LED, LOW);    //Gives a "long" LED signal when action is acknowledged
  delay(t2*40);
  digitalWrite(LED, HIGH);

} //End SetUp


//----------------Loop---------------------------------------------------OK

void loop()
{
  readIR();       //done
  codeDecode();   //done
  Record();       //done
  playBack();     //done
  stopMove();     //done
  movebyIR();     //done
  writetoSD();    //done
  deleteAll();    //done


}

//-------------------------------------------------------------OK
void readIR()
{
  if (irrecv.decode(&results))
  {
    codeValue = (results.value);
   Serial.print("codeValue = "); Serial.println(codeValue); 
    delay(100);    
    irrecv.resume(); // Receive the next value

  }
}

//-------------------------------------------------------------------OK
void   codeDecode()
{
    if (codeValue == 16738455) {servo1 = 12;} //"1" Servo1 Left
    if (codeValue == 16750695) {servo1 = 11;} //"2" Servo1 Right
    if (codeValue == 16724175) {servo2 = 22;} //"4" Servo2 Forward
    if (codeValue == 16718055) {servo2 = 21;} //"5" Servo2 Backwards
    if (codeValue == 16756815) {servo3 = 32;} //"3" Servo3 Up
    if (codeValue == 16743045) {servo3 = 31;} //"6" Servo3 Down
    if (codeValue == 16734885) {servo4 = 42;} //"9" Servo4 up
    if (codeValue == 16730805) {servo4 = 41;} //"0" Servo4down
    if (codeValue == 16716015) {servo5 = 51;} //"7" Servo5Left
    if (codeValue == 16726215) {servo5 = 52;} //"8" Servorighr
    if (codeValue == 16720605) {servo6 = 61;} //"<" Servo6 Open / Close
    if (codeValue == 16761405) {delAll = 1;} //">" Delete all both SD and RAM
    if (codeValue == 16754775) {writeSD = 1;} //"V" WritetoCD
    if (codeValue == 16736925) {nextStep = 1;} //"A" Next step
    if (codeValue == 16732845) {play = 1;} //"#" Play
    if (codeValue == 16712445) {Rec = 1;} //OK  position stored in arrary no
    if (codeValue == 16728765) {Stop = 1;} //"*" Stop
}
     

//-------------------------------------------------------------------OK
void Record()
{
  if (Rec == 1)
  {
  if(posCount<top-1)
  {
        for (int i =0; i<activeServos; i ++)
         {
         armPos[posCount][i] = armIs[i];
         }
        posCount = posCount + 1; posCountMax = posCount;


      stopRec();            //Stop moving servos when ""Rec" button is pressed
    } 
    blinkUp();
  }
  codeValue = 0;  //Reset input selection
  Rec = 0;        //Reset routine
}


//-----------------------------------------------------------------------

void playBack()
{
  int val =1;   //Robot will repeat program until velo speed is below setting and val=0
  if (play == 1)
  {
   posCount =0;

  blinkUp();

    while(posCount<posCountMax)
    {
      veloSpeed();
      Read(); 
      playServo();   // physically move arm to updated position, this is broken into small steps
     delay(velo);   // pause between moves 

    readIR();
   if (codeValue == 16728765) {blinkUp(); posCount= posCountMax; val=LOW; codeValue =0; play =0;} 
      if(velo > 40)
      {
      val=LOW;
      } 
      if ((posCount == posCountMax) && (val == HIGH)) 
      {
       posCount = 0; 
       }

    }
  }
  codeValue = 0;  //Reset input selection
  play = 0;       //Reset routine
}



//----------------------------------------------------------------------OK
void writetoSD()
{
    if(writeSD==1)
    {
  blinkUp();
      
      SD.remove("prgm.txt");
     Serial.println("Old prgm.txt is deleted from SD");
      posCount = 0;
      Robot = SD.open("prgm.txt", FILE_WRITE);


        if (Robot) 
        {

               Serial.println("New prgm is written to SD :");             
            while(posCount< posCountMax)
            {
              for(int i=0; i<activeServos; i++)
              {
              Robot.write(armPos[posCount][i]);

 //            Serial.print("Step =");Serial.print(posCount); Serial.print("  Servo ");Serial.print(i); Serial.print("   Value = ");
 //            Serial.println(armPos[posCount][i]);
              }
              posCount ++;   
                 
            }   

        // close the file:
        Robot.close();
        Serial.println("done.");
        } 
        else 
       {
        // if the file didn't open, print an error:
        Serial.println("error writing to prgm.txt");
       }


    }
codeValue =0;
writeSD = 0;
}
//--------------------------------------------------------------------------------------OK
void deleteAll()
  {
    if ( delAll == 1)  //Del button arraw down
  {
blinkUp();
      
      SD.remove("prgm.txt");
//      Serial.println("prgm.txt is deleted from SD");

      while(posCount<posCountMax)
      {
        for (int i=0; i<activeServos; i++)
        {
        armPos[posCount][i] = {00};
        }
        posCount ++;
      }
//      Serial.println("Flash memo is cleared and deleted");
      posCount = 0;
      posCountMax = 0;
  }
    codeValue = 0;      //Reset input selection
    delAll=0;           //Reset routine
  }



//----------------------------------------------------------------OK
void stopMove()
{
  if (Stop == 1)  //Stop button
  {

blinkUp();

    servo1 = servo2 = servo3 = servo4 = servo5= servo6 = 0;


    codeValue = 0;    //Reset input selection
    Stop = 0;         //reset routine
  }
}



//----------------------------------------------------------------------------OK

void movebyIR()
{
const int S1min = 18, S1max = 166, S2min = 20, S2max = 175, S3min = 60, S3max = 177, 
          S4min = 5, S4max = 172, S5min = 2, S5max = 180, S6min = 70, S6max = 90;
const int v = 1 ;   //Increments in for loops movebyIR
          
veloSpeed();


  //Servo1
  if ((servo1 == 11) && (armIs[0] > S1min))
  {
 blinkUp();

// Serial.print("Servo1 = "); Serial.println (armIs[0]);

    armIs[0] = armIs[0] - v;
    S1.write(armIs[0]);
    delay(velo);
  }

  if ((servo1 == 12) && (armIs[0] < S1max))
    {
blinkUp();

//Serial.print("Servo1 = "); Serial.println (armIs[0]);

    armIs[0] = armIs[0] + v;
    S1.write(armIs[0]);
    delay(velo);
  }

  //Servo2
  if ((servo2 == 21) && (armIs[1] > S2min))
  { 
blinkUp();
//Serial.print("Servo2  = "); Serial.println (armIs[1]);
    armIs[1] = armIs[1] - v;
    S2.write(armIs[1]);
    delay(velo);
  }

  if ((servo2 == 22) && (armIs[1] < S2max))
  {
blinkUp();
   
//Serial.print("Servo2  = "); Serial.println (armIs[1]);
    armIs[1] = armIs[1] + v;
    S2.write(armIs[1]);
    delay(velo);
  }

  //Servo3
  if ((servo3 == 31) && (armIs[2] > S3min))
  { 
blinkUp();
//Serial.print("Servo3  = "); Serial.println (armIs[2]);
    armIs[2] = armIs[2] - v;
    S3.write(armIs[2]);
    delay(velo);
  }

  if ((servo3 == 32) && (armIs[2] < S3max))
  {      
 blinkUp();
//Serial.print("Servo3  = "); Serial.println (armIs[2]);
    armIs[2] = armIs[2] + v;
    S3.write(armIs[2]);
    delay(velo);
  }

  //Servo 4
  if ((servo4 == 41) && (armIs[3] > S4min))
  {      
 blinkUp();
 //Serial.print("Servo4  = "); Serial.println (armIs[3]);
    armIs[3] = armIs[3] - v;
    S4.write(armIs[3]);
    delay(velo);
  }

  if ((servo4 == 42) && (armIs[3] < S4max))
  {      
blinkUp();
 //Serial.print("Servo4  = "); Serial.println (armIs[3]);
    armIs[3] = armIs[3] + v;
    S4.write(armIs[3]);
    delay(velo);
  }

  //Servo 5
  if ((servo5 == 51) && (armIs[4] > S5min))
  {      
blinkUp();
// Serial.print("Servo5  = "); Serial.println (armIs[4]);
    armIs[4] = armIs[4] - v;
    S5.write(armIs[4]);
    delay(velo);
  }

  if ((servo5 == 52) && (armIs[4] < S5max))
  {      
blinkUp();
 //Serial.print("Servo5  = "); Serial.println (armIs[4]);
    armIs[4] = armIs[4] + v;
    S5.write(armIs[4]);
    delay(velo);
  }

 
  //Servo 6
  if ((servo6 == 61) && (armIs[5] <= S6min))
  {      
blinkUp();

      while(armIs[5] <= S6max)
      {
 //    Serial.print("Servo6  = "); Serial.println (armIs[5]);
      armIs[5]++;
      S6.write(armIs[5]);
      delay(velo);
      }
     codeValue = 0;
  servo6 =0;   
  }

  if ((servo6 == 61) && (armIs[5] >= S6max))
  {      
blinkUp();
  
      while(armIs[5] >=S6min)
      {
 //   Serial.print("Servo6  = "); Serial.println (armIs[5]);
      armIs[5] --;
      S6.write(armIs[5]);
      delay(velo);
      }
  codeValue = 0;
  servo6 =0;
  }

}

//--------------------Sub Routine for veloSpeed------------------------------------
void veloSpeed()
{

  velo = analogRead(0);                   //Reading potentiometer value from pin A0
  velo = map(velo, 1024, 0, 50, 1);      //Map the value to a good velocity value - velo

} 

//-----------Sub Routine to stop move when OK is pressed for direct recording----------OK
void stopRec()
{
blinkUp();

    servo1 = servo2 = servo3 = servo4 = servo5 = servo6 = 0;
}

//-------------------Sub Routine to read and move from point to point----------OK
void Read()
{
int posReached;
  posReached = 0;
  for (int i = 0; i < activeServos ; i++ ) 
  {
     if ( armIs[i] > armPos[posCount][i]) armIs[i] = armIs[i] - 1; 
    if ( armIs[i] < armPos[posCount][i]) armIs[i] = armIs[i] + 1; 
    if ( armIs[i] == armPos[posCount][i]) posReached++; 
  }
    if (posReached == activeServos) posCount ++;  
}

//-----------------Sub Routine to move Servos to new position-------------OK
void playServo()
{  //Move all servos to new position
  S1.write(armIs[0]);
  S2.write(armIs[1]);
  S3.write(armIs[2]);
  S4.write(armIs[3]);
  S5.write(armIs[4]);
  S6.write(armIs[5]);
}


//----Sub Routine to give a flash when a button is operated--------OK
void blinkUp()
{
const int t2 =80;   //Length of LED Blink
  digitalWrite(LED, LOW);   
  delay(t2);                //Length of flash
  digitalWrite(LED, HIGH ); 
}

Credits

kjaeger

kjaeger

1 project • 0 followers
Contact

Comments

Add projectSign up / Login