Robot programming for real

Early 70thies, I joined the Trallfa Robot Division. Since than most of my time has been dedicated to robots. Many good memories from comapnies and people around the world. We had one of the first industial robots in poduction in the world. Even sold on licenses to Kobe in Japan.

The father of the robot - Ole Molaug became a good companien of mine.

As a thanks to Ole, I want to give him a replica of the TR2000 but now designed with a 3D printer and an Arduino pro mini and 6 off SG90.

So far, all good. I can by means of an IR system send codes to the Arduino and "jogg" the robot to the positions I want. My consern is now to be able to stare the value of each of the servos in an array. An array for each position I want the robot to reach and than recall the position either step by step or in a continous mode.

Enclosed is my unrefined sketch.

Special attention to the "void printposRec()"
At this stage I have all positions in variable "posS1, posS2 .... posS6"
I send a code from my IR and want to store the six positions as a point for later recall.
Further I want to be able to store several positions - at least 10 and be able to playback each point.

Any kind smart one out there guiding me in which directon to go??

#include <IRremote.h>
int RECV_PIN = 11;
IRrecv irrecv(RECV_PIN);
decode_results results;

#include <Servo.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;

int rec=0;
int play=0;
int delAll=0;
int delThis=0;
int nxtStp=0;
int Star=0;
int printRec=0;
int p=0;
char pointS1(p);

//Servo 0 position Min and Max position
int posS1 = 90;
int posS1min = 0;
int posS1max = 180;
int posS2 = 90;
int posS2min = 30;
int posS2max = 180;
int posS3 = 90;
int posS3min = 10;
int posS3max = 160;
int posS4 = 90;
int posS4min = 0;
int posS4max = 180;
int posS5 = 90;
int posS5min = 30;
int posS5max = 180;
int posS6 = 70;
int posS6min = 0;
int posS6max = 70;

//Which Servo to operate at a time in jogging mode
int servo1 = 0;
int servo2 = 0;
int servo3 = 0;
int servo4 = 0;
int servo5 = 0;
int servo6 = 0;


int v = 1;  //Speed of servo
int t = 40; //delay for diff purposes
unsigned long codeValue;  //IR Recived value

void setup() {

  Serial.begin(9600);
  Serial.println("Enabling IRin");
  irrecv.enableIRIn(); // Start the receiver
  delay(t);

  myservo1.attach(3);  // attaches the servo on pin 3 to the servo object
  myservo2.attach(5);  // attaches the servo on pin 5 to the servo object
  myservo3.attach(6);  // attaches the servo on pin 6 to the servo object
  myservo4.attach(10);  // attaches the servo on pin 10 to the servo object
  myservo5.attach(12);  // attaches the servo on pin12 to the servo object
  myservo6.attach(9);  // attaches the servo on pin 9 to the servo object


//Sets the robot to mid position before start up
  myservo1.write(posS1);
  myservo2.write(posS2);
  myservo3.write(posS3);
  myservo4.write(posS4);
  myservo5.write(posS5);
  myservo6.write(posS6);
  delay(t);
}


void loop() 
{
  readIR();       //done
  decodeIR();     //done
  movebyIR();     //done
  playback();      //to be done
  playStepbyStep(); //to be done
  printposRec();      //printing posSn + store posSn in array
  Stop();           //Stop routine and reset codeValue

}



void readIR()
{
  if (irrecv.decode(&results)) {
    codeValue = (results.value);
    Serial.print("codeValue ="); Serial.println(results.value, DEC);
    irrecv.resume(); // Receive the next value
  }
  delay(t);
}

void decodeIR()
{
  if (codeValue == 16738455) {servo1 = 11;} //"1" Servo1 Left
  if (codeValue == 16750695) {servo1 = 12;} //"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 = 41;} //"9" Servo4 up
  if (codeValue == 16730805) {servo4 = 42;} //"0" Servo4 Dowen
  if (codeValue == 16716015) {servo5 = 52;} //"7" Servo5 Left
  if (codeValue == 16726215) {servo5 = 51;} //"8" Servo5 Right
  if (codeValue == 16720605) {servo6 = 61;} //"<" Servo6 Open or close
  if (codeValue == 16761405) {nxtStp = 1;} //">" nxtStp
  if (codeValue == 16761405) {delAll = 1;} //"v" delAll
  if (codeValue == 16761405) {delThis = 1;} //"A" deleteThis
  if (codeValue == 16732845) {play = 1;} //"#" Play
  if (codeValue == 16712445) {printRec = 1;}  //OK printer ut servo position and store in arraryand inc row no
  if (codeValue == 16728765) {Star = 1;} //"*" Stop
}

void   movebyIR()
{
  //Servo1

  if ((servo1 == 11) && (posS1 > posS1min))
  {
    Serial.print("Servo1 = "); Serial.println (posS1);
    posS1 = posS1 - v;
    myservo1.write(posS1);
    delay(t);
  }


  if ((servo1 == 12) && (posS1 < posS1max))
  {
    Serial.print("Servo1 = "); Serial.println (posS1);
    posS1 = posS1 + v;
    myservo1.write(posS1);
    delay(t);
  }


  //Servo2

  if ((servo2 == 21) && (posS2 > posS2min))
  {
    Serial.print("Servo2  = "); Serial.println (posS2);
    posS2 = posS2 - v;
    myservo2.write(posS2);
    delay(t);
  }
  if ((servo2 == 22) && (posS2 < posS2max))
  {
    Serial.print("Servo2  = "); Serial.println (posS2);
    posS2 = posS2 + v;
    myservo2.write(posS2);
    delay(t);
  }


  //Servo3

  if ((servo3 == 31) && (posS3 > posS3min))
  {
    Serial.print("Servo3  = "); Serial.println (posS3);
    posS3 = posS3 - v;
    myservo3.write(posS3);
    delay(t);
  }

  if ((servo3 == 32) && (posS3 < posS3max))
  {
    Serial.print("Servo3  = "); Serial.println (posS3);
    posS3 = posS3 + v;
    myservo3.write(posS3);
    delay(t);
  }


  //Servo 4

  if ((servo4 == 41) && (posS4 > posS4min))
  {
    Serial.print("Servo4  = "); Serial.println (posS4);
    posS4 = posS4 - v;
    myservo4.write(posS4);
    delay(t);
  }

  if ((servo4 == 42) && (posS4 < posS4max))
  {
    Serial.print("Servo4  = "); Serial.println (posS4);
    posS4 = posS4 + v;
    myservo4.write(posS4);
    delay(t);
  }


  //Servo 5

  if ((servo5 == 51) && (posS5 > posS5min))
  {
    Serial.print("Servo5  = "); Serial.println (posS5);
    posS5 = posS5 - v;
    myservo5.write(posS5);
    delay(t);
  }

  if ((servo5 == 52) && (posS5 < posS5max))
  {
    Serial.print("Servo5  = "); Serial.println (posS5);
    posS5 = posS5 + v;
    myservo5.write(posS5);
    delay(t);
  }


  //Servo 6
  if ((servo6 == 61) && (posS6 == posS6max))
  {
    for (posS6 == posS6max; posS6 > posS6min; posS6 = posS6 - v * 10)
    {
      myservo6.write(posS6);
      delay(t);
    }
    servo6 = 0;
  }

  if ((servo6 == 61) && (posS6 == posS6min))
  {
    for (posS6 == posS6min; posS6 < posS6max; posS6 = posS6 + v * 10)
    {

      myservo6.write(posS6);
      delay(t);
    }

    servo6 = 0;
  }
}


void playback()
{
      if (play == 1 ) //You are in playBack Mode
      
    {
  /*   for (int (point[p]) = 0; (point[p]) < 5; (point[p]) = (point[p]) + 1)
      {
        myservo1.write(point[p]), 0;
         myservo2.write(point[p]), 1;
          myservo3.write(point[p]), 2;
           myservo4.write(point[p]), 3;
            myservo5.write(point[p]), 4;
             myservo6.write(point[p]), 5;
        delay(t);
      }*/
    }
}

void  playStepbyStep()
{

}


void printposRec()
{
  if (printRec == 1 )  //OK beeing pressed
  
  {
   Serial.println("you are now in rec mode mode");
p=1;
    int point(p),[6] = {(posS1), (posS2), (posS3), (posS4), (posS5), (posS6)};

  /* posS2(p)=posS2;
   posS3(p)=posS3;
   posS4(p)=posS4;
   posS5(p)=posS5;
   posS6(p)=posS6;               
 */ 
    Serial.print("Servo 1 = "); Serial.println(point(p)[0]);
    Serial.print("Servo 2 = "); Serial.println(point(p)[1]);
    Serial.print("Servo 3 = "); Serial.println(point(p)[2]);
    Serial.print("Servo 4 = "); Serial.println(point(p)[3]);
    Serial.print("Servo 5 = "); Serial.println(point(p)[4]);
    Serial.print("Servo 6 = "); Serial.println(point(p)[5]);


p++;
if(p>=1)
{
p=0;
codeValue=0;
    printRec=0;
  }
  }  
}

void Stop()
{
    if (Star == 1)  //Stop button 
  {
         Serial.println("You have now Stopped");
    servo1 = 0;
    servo2 = 0;
    servo3 = 0;
    servo4 = 0;
    servo5 = 0;
    servo6 = 0;
    codeValue = 0;
    Star=0;
  }
}
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;

Array time

I agree with @AWOL - you have lots of things that should go into arrays. Then you could probably reduce the main part of the code to 20 or 30 lines for all the servos and all the movements.

It would also be a good idea to read up about structs. You could have a struct for each servo that holds all its info - min. max, current position, whether it is to be jogged or not. And then you could have an array with one of those structs for each servo.

If nothing else it would make the code much easier to understand.

...R

you cannot initialize an array like this within a function:

    int point(p),[6] = {(posS1), (posS2), (posS3), (posS4), (posS5), (posS6)};

  /* posS2(p)=posS2;
   posS3(p)=posS3;

This is a bit dodgy, not sure what you are trying to do

int point(p),[6]

this is a function called point that takes an argument p

point(p)

this is an array called point that contains six elements:

point[6]

Perhaps spend a little time learning C++ and its syntax (and arrays).