Control 5 servos 3 joysticks recording coordinates arduino uno

Using Arduino Programming Questions
1

I am trying to control 5 servos (SG90) with 3 joysticks (generic analog) in arduino uno. I have followed the project " MeArm Robot - Recording of Coordinates (Minimalized Version)" by utilstudio which uses 4 servos and 2 joysticks. The code works well, the servos move with each joystick and recording-playing of coordinates are correct.

Problems come when I try to modify the code to add the 5th servo and the 3rd joystick. I have tried lot of modifications but result is more or less the same. One of servos does not work, or two servos move when moving only one axis of joystick and the recording and playing does not work at all.

I post both codes, the original which works well, and the modified which does not. Please if some clever mind can find the key to make the right changes in the code for the correct running. I know is difficult but i am sure somebody has to know what to do. Thank you very much:

This is the original code which works well

/* meArm analog joysticks version 1.5.4 - UtilStudio.com Dec 2018
   Uses two analogue joysticks and four servos.

   In version 1.5.4 was replaced an external resistors by internal pull up resistors.
   External LED diode was replaced with using internal "L" diode on Arduino UNO board.
   Some bugs was removed.

   First joystick moves gripper forwards, backwards, left and right,
   button start/stop recording positions.

   Second joystick moves gripper up, down, and closes and opens,
   button start/stop playing recorded positions.
   Press button for 2 seconds to autoplay.

   Pins:
   Arduino    Stick1    Stick2    Base   Shoulder  Elbow    Gripper   Record/
      GND       GND       GND    Brown     Brown   Brown     Brown    Auto play
       5V       VCC       VCC      Red       Red     Red       Red    LED
       A0       HOR
       A1       VER
       PD2      BUTT
       A2                 HOR
       A3                 VER
       PD3                BUTT
       11                       Yellow
       10                                 Yellow
        9                                         Yellow
        6                                                   Yellow
       13                                                             X
*/
#include <Servo.h>

bool repeatePlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int basePin = 11;       /* Base servo */
int shoulderPin = 10;   /* Shoulder servo */
int elbowPin = 9;       /* Elbow servo */
int gripperPin = 6;     /* Gripper servo */

int xdirPin = 0;        /* Base - joystick1*/
int ydirPin = 1;        /* Shoulder - joystick1 */
int zdirPin = 3;        /* Elbow - joystick2 */
int gdirPin = 2;        /* Gripper - joystick2 */

//int pinRecord = A4;     /* Button record - backward compatibility */
//int pinPlay = A5;       /* Button play  - backward compatibility */
int pinRecord = PD2;     /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13;   /* LED - indicates recording (light) or auto play mode (blink ones) */

const int buffSize = 512; /* Size of recording buffer */

int startBase = 90;
int startShoulder = 90;
int startElbow = 90;
int startGripper = 0;

int posBase = 90;
int posShoulder = 90;
int posElbow = 90;
int posGripper = 0;

int lastBase = 90;
int lastShoulder = 90;
int lastElbow = 90;
int lastGripper = 90;

int minBase = 0;
int maxBase = 150;
int minShoulder = 0;
int maxShoulder = 150;
int minElbow = 0;
int maxElbow = 150;
int minGripper = 0;
int maxGripper = 150;

const int countServo = 4;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servoBase;
Servo servoShoulder;
Servo servoElbow;
Servo servoGripper;

void setup() {
  Serial.begin(9600);

  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin, INPUT);
  pinMode(gdirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);

  StartPosition();

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatePlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatePlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatePlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);

  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;

  if (play)
  {
    if (playPos >= recPos) {
      playPos = 0;

      if (repeatePlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData = false;

    while (!endOfData)
    {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x3000;
      endOfData = data & 0x4000;

      switch (servoNumber)
      {
        case 0x0000:
          posBase = angle;
          break;

        case 0x1000:
          posShoulder = angle;
          break;

        case 0x2000:
          posElbow = angle;
          break;

        case 0x3000:
          posGripper = angle;
          dg = posGripper - lastGripper;
          break;
      }

      playPos++;
    }
  }

  if (posBase > maxBase) posBase = maxBase;
  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
  if (posElbow > maxElbow) posElbow = maxElbow;
  if (posGripper > maxGripper) posGripper = maxGripper;

  if (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder) posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);

  //  if (dg < -3.0) {
  //    posGripper = minGripper;
  //    servoGripper.write(posGripper);
  //    Serial.println(posGripper);
  //  }
  //  else if (dg > 3.0) {
  //    posGripper = maxGripper;
  //    servoGripper.write(posGripper);
  //    Serial.println(posGripper);
  //  }

  bool waitGripper = false;
  if (dg < 0) {
    posGripper = minGripper;
    waitGripper = true;
  }
  else if (dg > 0) {
    posGripper = maxGripper;
    waitGripper = true;
  }

  servoGripper.write(posGripper);
  if (play && waitGripper)
  {
    delay(1000);
  }

  //Serial.println(posGripper);

  if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lastBase != posBase)
        {
          buffAdd[buffPos] = posBase;
          buffPos++;
        }

        if (lastShoulder != posShoulder)
        {
          buffAdd[buffPos] = posShoulder | 0x1000;
          buffPos++;
        }

        if (lastElbow != posElbow)
        {
          buffAdd[buffPos] = posElbow | 0x2000;
          buffPos++;
        }

        if (lastGripper != posGripper)
        {
          buffAdd[buffPos] = posGripper | 0x3000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(posBase);
    Serial.print("\t");
    Serial.print(posShoulder);
    Serial.print("\t");
    Serial.print(posElbow);
    Serial.print("\t");
    Serial.print(posGripper);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lastBase = posBase;
  lastShoulder = posShoulder;
  lastElbow = posElbow;
  lastGripper = posGripper;

  if ( repeatePlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  };

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x3000;
    bool endOfData = data & 0x4000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angleBase = servoBase.read();
  int angleShoulder = servoShoulder.read();
  int angleElbow = servoElbow.read();
  int angleGripper = servoGripper.read();

  Serial.print(angleBase);
  Serial.print("\t");
  Serial.print(angleShoulder);
  Serial.print("\t");
  Serial.print(angleElbow);
  Serial.print("\t");
  Serial.print(angleGripper);
  Serial.println("\t");

  posBase = startBase;
  posShoulder = startShoulder;
  posElbow = startElbow;
  posGripper = startGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoGripper.write(posGripper);
}

This is the modified code which does not work well

((In my code joystick 1 controls servos 1 & 2 well; joystick 2 controls servo 4 well but when acting on servo 3 servo 4 moves at same time. Joystick 3 should control servo 5 but it does not. Record button works, play button works and loop option works but the move is not recorded well. There is record in servos 1.2.3 & 4 but when playing the sequence is not correct, besides the servo 4 gets trapped in loop mode despite loop mode is not activated))

#include <Servo.h>

bool repeatePlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int s1Pin = 11;       /* s1 servo */
int s2Pin = 10;      /* s2 servo */
int s3Pin = 9;       /* s3 servo */
int s4Pin = 6;       /* s4 servo */
int s5Pin = 7;        /*s5 servo */

int wdirPin = A0;        /* s1 - joystick1*/
int xdirPin = A1;        /* s2 - joystick1 */
int ydirPin = A3;        /* s3 - joystick2 */
int zdirPin = A2;        /* s4 - joystick2 */
int gdirPin = A4;        /* s5 - joystick3 */

//int pinRecord = A4;     /* Button record - backward compatibility */
//int pinPlay = A5;       /* Button play  - backward compatibility */
int pinRecord = PD2;     /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13;   /* LED - indicates recording (light) or auto play mode (blink ones) */

const int buffSize = 512; /* Size of recording buffer */

int starts1 = 90;
int starts2 = 90;
int starts3 = 90;
int starts4 = 0;
int starts5 = 90;

int poss1 = 90;
int poss2 = 90;
int poss3 = 90;
int poss4 = 0;
int poss5 = 90;

int lasts1 = 90;
int lasts2 = 90;
int lasts3 = 90;
int lasts4 = 90;
int lasts5 = 90;

int mins1 = 0;
int maxs1 = 150;
int mins2 = 0;
int maxs2 = 150;
int mins3 = 0;
int maxs3 = 150;
int mins4 = 0;
int maxs4 = 150;
int mins5 = 0;
int maxs5 = 150;

const int countServo = 5;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servos1;
Servo servos2;
Servo servos3;
Servo servos4;
Servo servos5;

void setup() {
  Serial.begin(9600);

  pinMode(wdirPin, INPUT);
  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin, INPUT);
  pinMode(gdirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servos1.attach(s1Pin);
  servos2.attach(s2Pin);
  servos3.attach(s3Pin);
  servos4.attach(s4Pin);
  servos5.attach(s5Pin);

  StartPosition();

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatePlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatePlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatePlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dw = map(analogRead(wdirPin), 0, 1023, -5.0, 5.0);
  float dx = map(analogRead(xdirPin), 0, 1023, 5.0, -5.0);
  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);

  if (abs(dw) < 1.5) dw = 0;
  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  poss1 += dw;
  poss2 += dx;
  poss3 += dy;
  poss4 += dz;
  poss5 += dg;

  if (play)
  {
    if (playPos >= recPos) {
      playPos = 0;

      if (repeatePlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData = false;

    while (!endOfData)
    {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x3000;
      endOfData = data & 0x4000;

      switch (servoNumber)
      {
        case 0x0000:
          poss1 = angle;
          break;

        case 0x1000:
          poss2 = angle;
          break;

        case 0x2000:
          poss3 = angle;
          break;

        case 0x3000:
          poss4 = angle;
          dz = poss4 - lasts4;
          break;

        case 0x4000:
          poss5 = angle;
          dg = poss5 - lasts5;
          break;
      }

      playPos++;
    }
  }

  if (poss1 > maxs1) poss1 = maxs1;
  if (poss2 > maxs2) poss2 = maxs2;
  if (poss3 > maxs3) poss3 = maxs3;
  if (poss4 > maxs4) poss4 = maxs4;
  if (poss5 > maxs5) poss5 = maxs5;

  if (poss1 < mins1) poss1 = mins1;
  if (poss2 < mins2) poss2 = mins2;
  if (poss3 < mins3) poss3 = mins3;
  if (poss4 < mins4) poss4 = mins4;
  if (poss5 < mins5) poss5 = mins5;

  servos1.write(poss1);
  servos2.write(poss2);
  servos3.write(poss3);

  //  if (dg < -3.0) {
  //    poss4 = mins4;
  //    servos4.write(poss4);
  //    Serial.println(poss4);
  //  }
  //  else if (dg > 3.0) {
  //    poss4 = maxs4;
  //    servos4.write(poss4);
  //    Serial.println(poss4);
  //  }

  bool waits4 = false;
  if (dz < 0) {
    poss4 = mins4;
    waits4 = true;
  }
  else if (dz > 0) {
    poss4 = maxs4;
    waits4 = true;
  }

  servos4.write(poss4);
  if (play && waits4)
  {
    delay(1000);
  }

  //Serial.println(poss4);

  if ((lasts1 != poss1) | (lasts2 != poss2) | (lasts3 != poss3) | (lasts4 != poss4))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lasts1 != poss1)
        {
          buffAdd[buffPos] = poss1;
          buffPos++;
        }

        if (lasts2 != poss2)
        {
          buffAdd[buffPos] = poss2 | 0x1000;
          buffPos++;
        }

        if (lasts3 != poss3)
        {
          buffAdd[buffPos] = poss3 | 0x2000;
          buffPos++;
        }

        if (lasts4 != poss4)
        {
          buffAdd[buffPos] = poss4 | 0x3000;
          buffPos++;
        }

        if (lasts5 != poss5)
        {
          buffAdd[buffPos] = poss5 | 0x4000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x5000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(poss1);
    Serial.print("\t");
    Serial.print(poss2);
    Serial.print("\t");
    Serial.print(poss3);
    Serial.print("\t");
    Serial.print(poss4);
    Serial.print("\t");
    Serial.print(poss5);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lasts1 = poss1;
  lasts2 = poss2;
  lasts3 = poss3;
  lasts4 = poss4;
  lasts5 = poss5;

  if ( repeatePlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  };

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x3000;
    bool endOfData = data & 0x4000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angles1 = servos1.read();
  int angles2 = servos2.read();
  int angles3 = servos3.read();
  int angles4 = servos4.read();
  int angles5 = servos5.read();

  Serial.print(angles1);
  Serial.print("\t");
  Serial.print(angles2);
  Serial.print("\t");
  Serial.print(angles3);
  Serial.print("\t");
  Serial.print(angles4);
  Serial.println("\t");
  Serial.print(angles5);
  Serial.println("\t");

  poss1 = starts1;
  poss2 = starts2;
  poss3 = starts3;
  poss4 = starts4;
  poss5 = starts5;
  

  servos1.write(poss1);
  servos2.write(poss2);
  servos3.write(poss3);
  servos4.write(poss4);
  servos5.write(poss5);
}
2

Hello, do yourself a favour and please read How to get the best out of this forum and post accordingly (including code with code tags and necessary documentation for your ask like your exact circuit and power supply, links to components etc).

3

Welcome to the forum

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the < CODE/ > icon above the compose window) to make it easier to read and copy for examination

https://forum.arduino.cc/t/how-to-get-the-best-out-of-this-forum

Please post your full sketch, using code tags when you do. This prevents parts of it being interpreted as HTML coding and makes it easier to copy for examination

In my experience the easiest way to tidy up the code and add the code tags is as follows
Start by tidying up your code by using Tools/Auto Format in the IDE to make it easier to read. Then use Edit/Copy for Forum and paste what was copied in a new reply. Code tags will have been added to the code to make it easy to read in the forum thus making it easier to provide help.

4

thanks for the advice. I posted in a little hurry and could not get by with text editor. I hope it can be understood now so that someone can help me. Thank you

5

Hello astorgallionspain

Welcome to the world's best Arduino forum ever.

I assume that you have written the programme by yourself, then it is quite easy to find the error.

There's a trick to figuring out why something isn't working:

Use a logic analyzer to see what happens.
Put Serial.print statements at various places in the code as diagnostic prints to see the values of variables, especially ones that control the servos, and determine whether they meet your expectations.

Have a nice day and enjoy coding in C++.

6

this is the typical problem when not using arrays. There's redundant code for each element: Base, soulder, elbow and gripper. to add another element, you now need to locate all the pieces of code that affect an element and duplicate it for the new element

the high school robotics students are working on code to control a multi-segment arm and just added a function (Java) for each element

    public double changePos(boolean backwards, boolean forwards, double ang, Servo servo){
            if(backwards)  {
                if(ang > 0)
                    ang-=servoInc;
            }
            else if (forwards){
                if(ang < 180)
                    ang+=servoInc;
            }
            servo.setPosition(ang/180.);
            return ang;
        }
    // -----------------------------------------------
    private void servos() {
       sh.setPosition(angSh/180.);
        angSh = changePos(gamepad1.cross,     gamepad1.triangle,   angSh, sh);
        angEl = changePos(gamepad1.dpad_down, gamepad1.dpad_up,    angEl, el);
        angTo = changePos(gamepad1.dpad_left, gamepad1.dpad_right, angTo, to);
        angWr = changePos(gamepad1.circle,    gamepad1.square,     angWr, wr);

in this code, the angles of each servo are initialize to 0.5, the servo values range from 0-1. So the servo get initialized to that 0.5 value when the functions are called for each and the angles remain unchanged

7

Which ones?

8

thank you for your reply. I used the original code and changed some parameters to see if it suits to 5 servos and 3 joysticks. I have no knowledge whatsoever in programming, so what you tell me sounds to me like japanese. I post the matter because maybe someone knows what to change in the code, or maybe there is another utterly different code able to control 5 servos with 3 joysticks with record and playing of coordinates. This is obviously is a very difficult project, in fact i have been searching the web and found nothing, the closest is the 4 servos 2 joysticks. Appreciate your help but I will not be able to fix this code in 100 years with my little knowledge. What i ask is if someone is able to get a working code either by modifying the 4 servo -2 joysticks code or with a new unknown code. Thank you

9

look this over

not sure about buff and any record/playback capability

/* meArm analog joysticks version 1.5.4 - UtilStudio.com Dec 2018
Uses two analogue joysticks and four servos.

In version 1.5.4 was replaced an external resistors by internal pull up resistors.
External LED diode was replaced with using internal "L" diode on Arduino UNO board.
Some bugs was removed.

First joystick moves gripper forwards, backwards, left and right,
button start/stop recording positions.

Second joystick moves gripper up, down, and closes and opens,
button start/stop playing recorded positions.
Press button for 2 seconds to autoplay.

Pins:
Arduino  Stick1    Stick2    Base   Shoulder  Elbow    Gripper   Record/
GND       GND       GND    Brown     Brown   Brown     Brown    Auto play
5V       VCC       VCC      Red       Red     Red       Red    LED
A0       HOR
A1       VER
PD2      BUTT
A2                 HOR
A3                 VER
PD3                BUTT
11                       Yellow
10                                 Yellow
9                                         Yellow
6                                                   Yellow
13                                                             X
*/

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

struct X {
    const int   PinServo;
    const int   PinJoy;

    int         pos;
    int         posMin;
    int         posMax;
    int         id;

    const char *desc;

    int         posLast;
    Servo       servo;
}
tbl [] = {
    { 11,  0, 90,   0,  180, 0x1000, "Base" },
    { 10,  1, 90,   0,  150, 0x2000, "Shoulder" },
    {  9,  3, 90,   0,  150, 0x3000, "Elbow" },
    {  6,  2, 90,   0,  150, 0x4000, "Gripper" },
};
const int countServo = sizeof (tbl)/sizeof(X);

// -------------------------------------
bool repeatePlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int pinRecord    = PD2; 
int pinPlay      = PD3; 
int pinLedRecord = 13;

const int buffSize = 512; /* Size of recording buffer */

// -------------------------------------
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay   = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast   = LOW;

bool record = false;
bool play   = false;
bool debug  = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

void PrintBuffer   ();
void StartPosition ();
void updateServos  ();

// -----------------------------------------------------------------------------
void setup ()
{
    Serial.begin (9600);

    for (int n = 0; n < countServo; n++)  {
        pinMode (tbl [n].PinJoy, INPUT);
        tbl [n].servo.attach (tbl [n].PinServo);
        
    }

    pinMode (pinRecord, INPUT_PULLUP);
    pinMode (pinPlay, INPUT_PULLUP);
    pinMode (pinLedRecord, OUTPUT);

    digitalWrite (pinLedRecord, HIGH);
    delay (1000);
    digitalWrite (pinLedRecord, LOW);

    printf ("%s:\n", __func__);
}

// -----------------------------------------------------------------------------
void loop ()
{
 // printf ("%s:\n", __func__);

    buttonRecord = digitalRead (pinRecord);
    buttonPlay   = digitalRead (pinPlay);

    //  Serial.print (buttonRecord);
    //  Serial.print ("\t");
    //  Serial.println (buttonPlay);
    //  for testing purposes

    if (buttonPlay == LOW) {
        printf (" %s: play\n", __func__);
        buttonPlayCount++;

        if (buttonPlayCount >= buttonPlayDelay) {
            repeatePlaying = true;
        }
    }
    else
        buttonPlayCount = 0;

    if (buttonPlay != buttonPlayLast) {
        if (record) {
            record = false;
        }

        if (buttonPlay == LOW) {
            play = !play;
            repeatePlaying = false;

            if (play) {
                StartPosition ();
            }
        }
    }

    if (buttonRecord != buttonRecordLast) {
        if (buttonRecord == LOW) {
            record = !record;

            if (record) {
                play = false;
                repeatePlaying = false;
                recPos = 0;
            }
            else {
                if (debug) PrintBuffer();
            }
        }
    }

    buttonPlayLast = buttonPlay;
    buttonRecordLast = buttonRecord;

    // -------------------------------------
    if (play) {
        if (playPos >= recPos) {
            playPos = 0;

            if (repeatePlaying) {
                delay (delayBetweenCycles);
                StartPosition ();
            }
            else {
                play = false;
            }
        }

        bool endOfData = false;

        while (! endOfData) {
            if (playPos >= buffSize - 1)
                break;

            if (playPos >= recPos)
                break;

            int data = buff[playPos];
            int angle = data & 0xFFF;
            int servoNumber = data & 0x3000;
            endOfData = data & 0x4000;

            for (int n = 0; n < countServo; n++)  {
                if (servoNumber == tbl [n].id)
                    tbl [n].pos = angle;
            }

            playPos++;
        }
    }

    // -------------------------------------
#if 0
        // what is this doing? adding to buff is position changed?

    if (      (lastBase  != posBase)  | (lastShoulder != posShoulder)
            | (lastElbow != posElbow) | (lastGripper != posGripper)) {
        if (record) {
            if (recPos < buffSize - countServo) {
                int buffPos = 0;

                for (int n = 0; n < countServo; n++)  {
                    if (tbl [n].posLast != tbl [n].pos)  {
                        tbl [n].posLast  = tbl [n].pos;
                    buffAdd [buffPos++] = tbl [n].pos;
                }

                buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000;

                for (int i = 0; i < buffPos; i++) {
                    buff[recPos + i] = buffAdd[i];
                }

                recPos += buffPos;
            }
        }

        command = "Manual";
        printPos = 0;

        if (play) {
            command = "Play";
            printPos = playPos;
        }
        else if (record) {
            command = "Record";
            printPos = recPos;
        }

        Serial.print (command);
        Serial.print ("\t");
        Serial.print (printPos);
        Serial.print ("\t");
        Serial.print (posBase);
        Serial.print ("\t");
        Serial.print (posShoulder);
        Serial.print ("\t");
        Serial.print (posElbow);
        Serial.print ("\t");
        Serial.print (posGripper);
        Serial.print ("\t");
        Serial.print (record);
        Serial.print ("\t");
        Serial.print (play);
        Serial.println ();
    }
#endif

    if ( repeatePlaying) {
        ledLight = !ledLight;
    }
    else {
        if (ledLight) {
            ledLight = false;
        }

        if (record) {
            ledLight = true;
        }
    };

    updateServos ();

    digitalWrite (pinLedRecord, ledLight);
    delay (50);

}

// -----------------------------------------------------------------------------
void
StartPosition ()
{
}

// -----------------------------------------------------------------------------
void
updateServos ()
{
    for (int n = 0; n < countServo; n++)  {
        float dPos = map (analogRead (tbl [n].PinJoy), 0, 1023, -5.0, 5.0);

        if (abs(dPos) < 1.5)
            dPos = 0;
        tbl [n].pos += dPos;
        if (tbl [n].pos > tbl [n].posMax)
            tbl [n].pos = tbl [n].posMax;
        if (tbl [n].pos < tbl [n].posMin)
            tbl [n].pos = tbl [n].posMin;

        tbl [n].servo.write (tbl [n].pos);
    }
}

// -----------------------------------------------------------------------------
void PrintBuffer ()
{
    for (int i = 0; i < recPos; i++)
    {
        int data = buff[i];
        int angle = data & 0xFFF;
        int servoNumber = data & 0x3000;
        bool endOfData = data & 0x4000;

        Serial.print ("Servo=");
        Serial.print (servoNumber);
        Serial.print ("\tAngle=");
        Serial.print (angle);
        Serial.print ("\tEnd=");
        Serial.print (endOfData);
        Serial.print ("\tData=");
        Serial.print (data, BIN);
        Serial.println ();
    }
}
10

thank you but this code is 4 servos. The 4 servos and 2 joysticks code works everything well

11

Thanks for your reply. I updated the thread with your question so that other people can see from the beginning.

12

yes, it's for you to add the addition servo/joy-sticks

13

Add stuff slowly and test. Add a new servo and recompile - does it work? Attach it in setup, does it still work. Keep going until you break it. and hopefully, the last thing you changed will give a clue as to what went wrong.

Rinse & repeat until complete.

14

I didn't dive deep into the program but try replacing every

servoNumber = data & 0x3000;

with

servoNumber = data & 0x7000;

and every

endOfData = data & 0x4000;

with

endOfData = data & 0x8000;

and

buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x5000;

with

buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

EDIT (explanation) :
The crucial part is at lines 215-218 (and elsewhere too) of your modified code :

To understand the 0x (which means hexadecimal or HEX) values, you need to notice that data (and buff[] alike) is a 16 bit integer (bits are numbered from 0 to 15) used to hold not only the servo position (lower 12 bits allowing up to 2^12 or 4096 different values) but also the servo number (was 2 bit (from 12 to 13) for up to 2^2 = 4 different servos modified to 3 bit (from 12 to 14) for up to 2^3 = 8 different servos) and bit 14 (in the original program, was (should have been) shifted to position 15 in the 5 to 8 servos version) is used to indicate the end of the valid data, the rest of the array (in the case of buff[]) being garbage, 'unknown value'.

The above code breaks this 16-bit integer into its different parts using bit masking method (interesting Wikipedia link).

angle = data & 0xFFF; // <- or preferably 0x0FFF : 4 HEX digits !!!

gets the 12 lower bits as 0x0FFF is equivalent to 0b0000 1111 1111 1111 in binary.

servoNumber = data & 0x3000;

gets bits 12 and 13 as 0x3000 is equivalent to 0b0011 0000 0000 0000 in binary and should be modified to

servoNumber = data & 0x7000;

to get bit 12 to 14 as 0x7000 is equivalent to 0b0111 0000 0000 0000 in binary.

Similarly,

endOfData = data & 0x4000;

gets bit 14 as 0x4000 is equivalent to 0b0100 0000 0000 0000 in binary and should be modified to

endOfData = data & 0x8000;

to get bit 15 as 0x8000 is equivalent to 0b1000 0000 0000 0000 in binary.
Any 0x number in your program should be adjusted (if needed) accordingly.

15

thank you very much Etienne 74. We have a little progress here!!!! The situation after applying your changes is as follow: joystick 1, one axis moves S1, the other axis moves S2 & S3 at same time. Joystick 2, one axis moves S4, the other axis moves S3 but slower than normal (like half normal speed). Joystick 3 does not move S5, besides when moving axis, RX led in the board does not light so is like is not receiving signal at all from joystick 3, consequently S5 will not move. The good part is that recording and playing works well. So next step is to fix the mess with Joystick 3 and servos. Thanks for your contribution

Code with your changes:


#include <Servo.h>

bool repeatePlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int s1Pin = 11;       /* s1 servo */
int s2Pin = 10;      /* s2 servo */
int s3Pin = 9;       /* s3 servo */
int s4Pin = 6;       /* s4 servo */
int s5Pin = 7;        /*s5 servo */

int wdirPin = A0;        /* s1 - joystick1*/
int xdirPin = A1;        /* s2 - joystick1 */
int ydirPin = A3;        /* s3 - joystick2 */
int zdirPin = A2;        /* s4 - joystick2 */
int gdirPin = A4;        /* s5 - joystick3 */

//int pinRecord = A4;     /* Button record - backward compatibility */
//int pinPlay = A5;       /* Button play  - backward compatibility */
int pinRecord = PD2;     /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13;   /* LED - indicates recording (light) or auto play mode (blink ones) */

const int buffSize = 512; /* Size of recording buffer */

int starts1 = 90;
int starts2 = 90;
int starts3 = 90;
int starts4 = 0;
int starts5 = 90;

int poss1 = 90;
int poss2 = 90;
int poss3 = 90;
int poss4 = 0;
int poss5 = 90;

int lasts1 = 90;
int lasts2 = 90;
int lasts3 = 90;
int lasts4 = 90;
int lasts5 = 90;

int mins1 = 0;
int maxs1 = 150;
int mins2 = 0;
int maxs2 = 150;
int mins3 = 0;
int maxs3 = 150;
int mins4 = 0;
int maxs4 = 150;
int mins5 = 0;
int maxs5 = 150;

const int countServo = 5;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servos1;
Servo servos2;
Servo servos3;
Servo servos4;
Servo servos5;

void setup() {
  Serial.begin(9600);

  pinMode(wdirPin, INPUT);
  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin, INPUT);
  pinMode(gdirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servos1.attach(s1Pin);
  servos2.attach(s2Pin);
  servos3.attach(s3Pin);
  servos4.attach(s4Pin);
  servos5.attach(s5Pin);

  StartPosition();

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatePlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatePlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatePlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dw = map(analogRead(wdirPin), 0, 1023, -5.0, 5.0);
  float dx = map(analogRead(xdirPin), 0, 1023, 5.0, -5.0);
  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);

  if (abs(dw) < 1.5) dw = 0;
  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  poss1 += dw;
  poss2 += dx;
  poss3 += dy;
  poss4 += dz;
  poss5 += dg;

  if (play)
  {
    if (playPos >= recPos) {
      playPos = 0;

      if (repeatePlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData = false;

    while (!endOfData)
    {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x7000;
      endOfData = data & 0x8000;

      switch (servoNumber)
      {
        case 0x0000:
          poss1 = angle;
          break;

        case 0x1000:
          poss2 = angle;
          break;

        case 0x2000:
          poss3 = angle;
          break;

        case 0x3000:
          poss4 = angle;
          dz = poss4 - lasts4;
          break;

        case 0x4000:
          poss5 = angle;
          dg = poss5 - lasts5;
          break;
      }

      playPos++;
    }
  }

  if (poss1 > maxs1) poss1 = maxs1;
  if (poss2 > maxs2) poss2 = maxs2;
  if (poss3 > maxs3) poss3 = maxs3;
  if (poss4 > maxs4) poss4 = maxs4;
  if (poss5 > maxs5) poss5 = maxs5;

  if (poss1 < mins1) poss1 = mins1;
  if (poss2 < mins2) poss2 = mins2;
  if (poss3 < mins3) poss3 = mins3;
  if (poss4 < mins4) poss4 = mins4;
  if (poss5 < mins5) poss5 = mins5;

  servos1.write(poss1);
  servos2.write(poss2);
  servos3.write(poss3);

  //  if (dg < -3.0) {
  //    poss4 = mins4;
  //    servos4.write(poss4);
  //    Serial.println(poss4);
  //  }
  //  else if (dg > 3.0) {
  //    poss4 = maxs4;
  //    servos4.write(poss4);
  //    Serial.println(poss4);
  //  }

  bool waits4 = false;
  if (dz < 0) {
    poss4 = mins4;
    waits4 = true;
  }
  else if (dz > 0) {
    poss4 = maxs4;
    waits4 = true;
  }

  servos4.write(poss4);
  if (play && waits4)
  {
    delay(1000);
  }

  //Serial.println(poss4);

  if ((lasts1 != poss1) | (lasts2 != poss2) | (lasts3 != poss3) | (lasts4 != poss4))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lasts1 != poss1)
        {
          buffAdd[buffPos] = poss1;
          buffPos++;
        }

        if (lasts2 != poss2)
        {
          buffAdd[buffPos] = poss2 | 0x1000;
          buffPos++;
        }

        if (lasts3 != poss3)
        {
          buffAdd[buffPos] = poss3 | 0x2000;
          buffPos++;
        }

        if (lasts4 != poss4)
        {
          buffAdd[buffPos] = poss4 | 0x3000;
          buffPos++;
        }

        if (lasts5 != poss5)
        {
          buffAdd[buffPos] = poss5 | 0x4000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(poss1);
    Serial.print("\t");
    Serial.print(poss2);
    Serial.print("\t");
    Serial.print(poss3);
    Serial.print("\t");
    Serial.print(poss4);
    Serial.print("\t");
    Serial.print(poss5);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lasts1 = poss1;
  lasts2 = poss2;
  lasts3 = poss3;
  lasts4 = poss4;
  lasts5 = poss5;

  if ( repeatePlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  };

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x7000;
    bool endOfData = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angles1 = servos1.read();
  int angles2 = servos2.read();
  int angles3 = servos3.read();
  int angles4 = servos4.read();
  int angles5 = servos5.read();

  Serial.print(angles1);
  Serial.print("\t");
  Serial.print(angles2);
  Serial.print("\t");
  Serial.print(angles3);
  Serial.print("\t");
  Serial.print(angles4);
  Serial.println("\t");
  Serial.print(angles5);
  Serial.println("\t");

  poss1 = starts1;
  poss2 = starts2;
  poss3 = starts3;
  poss4 = starts4;
  poss5 = starts5;
  

  servos1.write(poss1);
  servos2.write(poss2);
  servos3.write(poss3);
  servos4.write(poss4);
  servos5.write(poss5);
}
16

Servo S5 should be connected to PWM capable pin, which, on the UNO (R3 and earlier), Nano and Mini are pins 3, 5, 6, 9, 10 & 11.
Use

int s5Pin = 5;          /* s5 servo */ // EDIT: no need for PWM capable pin, see post #28

and connect servo 5 accordingly.

And I don't see any

  servoS5.write(posS5);

in the loop(). Add one just above

  if ((lastS1 != posS1) | (lastS2 != posS2) | (lastS3 != posS3) | (lastS4 != posS4))

and report back.

Do you use real analog Joysticks or are they "Hat Switch like" analog Joysticks (up, down, left, right, center) ?

Here is a Wokwi simulation with true "analog joysticks" and here is one with "Hat Switch like" analog Joysticks.

1 Like
17

guess it was lost on you that you would add an additional line to tbl[]

but other parts of the code, the play/record, were confusing to me

18

i've been looking over this code, trying to understand it's play/record mechanism.

looks like there may not be a servo change every record period. how does period between non-changes get identified in buff[]?

considering that buff is only 512 elements, seems wasteful to capture non-changes

19

Thank you very much Etienne.
I have changed s5 to pin 5.

servoS5.write(posS5);

it is just in the last line of code.

This part i do not understand well what do you mean...

if ((lastS1 != posS1) | (lastS2 != posS2) | (lastS3 != posS3) | (lastS4 != posS4))

Is it add same with s5 after s4???
The joystick i am using is labelled with wh-504 i guess is same as the hat swithc like as you call.

20

Thank you very much Etienne
i have tried with your code posted in the simulator:

#include <Servo.h>

bool repeatPlaying = false; /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int s1Pin = 11;         /* s1 servo */
int s2Pin = 10;         /* s2 servo */
int s3Pin = 9;          /* s3 servo */
int s4Pin = 6;          /* s4 servo */
int s5Pin = 5;          /* s5 servo */

int wDirPin = A0;       /* s1 - joystick1*/
int xDirPin = A1;       /* s2 - joystick1 */
int yDirPin = A3;       /* s3 - joystick2 */
int zDirPin = A2;       /* s4 - joystick2 */
int gDirPin = A4;       /* s5 - joystick3 */

int pinRecord = 2;      /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay   = 3;      /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = 13;  /* LED - indicates recording (light) or auto play mode (blink ones) */

const int buffSize = 512; /* Size of recording buffer */

int startS1 = 90;
int startS2 = 90;
int startS3 = 90;
int startS4 = 0;
int startS5 = 90;

int posS1 = 90;
int posS2 = 90;
int posS3 = 90;
int posS4 = 0;
int posS5 = 90;

int lastS1 = 90;
int lastS2 = 90;
int lastS3 = 90;
int lastS4 = 90;
int lastS5 = 90;

int minS1 = 0;
int maxS1 = 150;
int minS2 = 0;
int maxS2 = 150;
int minS3 = 0;
int maxS3 = 150;
int minS4 = 0;
int maxS4 = 150;
int minS5 = 0;
int maxS5 = 150;

const int countServo = 5;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servoS1;
Servo servoS2;
Servo servoS3;
Servo servoS4;
Servo servoS5;

void setup() {
  Serial.begin(115200);

  pinMode(wDirPin, INPUT);
  pinMode(xDirPin, INPUT);
  pinMode(yDirPin, INPUT);
  pinMode(zDirPin, INPUT);
  pinMode(gDirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servoS1.attach(s1Pin);
  servoS2.attach(s2Pin);
  servoS3.attach(s3Pin);
  servoS4.attach(s4Pin);
  servoS5.attach(s5Pin);

  StartPosition();

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatPlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatPlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatPlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dw = map(analogRead(wDirPin), 0, 1023, -5.0, 5.0);
  float dx = map(analogRead(xDirPin), 0, 1023, 5.0, -5.0);
  float dy = map(analogRead(yDirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zDirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gDirPin), 0, 1023, 5.0, -5.0);

  if (abs(dw) < 1.5) dw = 0;
  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posS1 += dw;
  posS2 += dx;
  posS3 += dy;
  posS4 += dz;
  posS5 += dg;

  if (play)
  {
    if (playPos >= recPos) {
      playPos = 0;

      if (repeatPlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData = false;

    while (!endOfData)
    {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x7000;
      endOfData = data & 0x8000;

      switch (servoNumber)
      {
        case 0x0000:
          posS1 = angle;
          break;

        case 0x1000:
          posS2 = angle;
          break;

        case 0x2000:
          posS3 = angle;
          break;

        case 0x3000:
          posS4 = angle;
          dz = posS4 - lastS4;
          break;

        case 0x4000:
          posS5 = angle;
          dg = posS5 - lastS5;
          break;
      }

      playPos++;
    }
  }

  if (posS1 > maxS1) posS1 = maxS1;
  if (posS2 > maxS2) posS2 = maxS2;
  if (posS3 > maxS3) posS3 = maxS3;
  if (posS4 > maxS4) posS4 = maxS4;
  if (posS5 > maxS5) posS5 = maxS5;

  if (posS1 < minS1) posS1 = minS1;
  if (posS2 < minS2) posS2 = minS2;
  if (posS3 < minS3) posS3 = minS3;
  if (posS4 < minS4) posS4 = minS4;
  if (posS5 < minS5) posS5 = minS5;

  servoS1.write(posS1);
  servoS2.write(posS2);
  servoS3.write(posS3);

  //  if (dg < -3.0) {
  //    posS4 = minS4;
  //    servoS4.write(posS4);
  //    Serial.println(posS4);
  //  }
  //  else if (dg > 3.0) {
  //    posS4 = maxS4;
  //    servoS4.write(posS4);
  //    Serial.println(posS4);
  //  }

  bool waitS4 = false;
  if (dz < 0) {
    posS4 = minS4;
    waitS4 = true;
  }
  else if (dz > 0) {
    posS4 = maxS4;
    waitS4 = true;
  }

  servoS4.write(posS4);
  if (play && waitS4)
  {
    delay(1000);
  }

  //Serial.println(posS4);

  servoS5.write(posS5);

  if ((lastS1 != posS1) | (lastS2 != posS2) | (lastS3 != posS3) | (lastS4 != posS4))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lastS1 != posS1)
        {
          buffAdd[buffPos] = posS1;
          buffPos++;
        }

        if (lastS2 != posS2)
        {
          buffAdd[buffPos] = posS2 | 0x1000;
          buffPos++;
        }

        if (lastS3 != posS3)
        {
          buffAdd[buffPos] = posS3 | 0x2000;
          buffPos++;
        }

        if (lastS4 != posS4)
        {
          buffAdd[buffPos] = posS4 | 0x3000;
          buffPos++;
        }

        if (lastS5 != posS5)
        {
          buffAdd[buffPos] = posS5 | 0x4000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(posS1);
    Serial.print("\t");
    Serial.print(posS2);
    Serial.print("\t");
    Serial.print(posS3);
    Serial.print("\t");
    Serial.print(posS4);
    Serial.print("\t");
    Serial.print(posS5);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lastS1 = posS1;
  lastS2 = posS2;
  lastS3 = posS3;
  lastS4 = posS4;
  lastS5 = posS5;

  if ( repeatPlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  }

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x7000;
    bool endOfData = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angles1 = servoS1.read();
  int angles2 = servoS2.read();
  int angles3 = servoS3.read();
  int angles4 = servoS4.read();
  int angles5 = servoS5.read();

  Serial.print(angles1);
  Serial.print("\t");
  Serial.print(angles2);
  Serial.print("\t");
  Serial.print(angles3);
  Serial.print("\t");
  Serial.print(angles4);
  Serial.println("\t");
  Serial.print(angles5);
  Serial.println("\t");

  posS1 = startS1;
  posS2 = startS2;
  posS3 = startS3;
  posS4 = startS4;
  posS5 = startS5;
  

  servoS1.write(posS1);
  servoS2.write(posS2);
  servoS3.write(posS3);
  servoS4.write(posS4);
  servoS5.write(posS5);
}

In the simulator works well, but when loading to arduino uno there are still some problems. However there are also more progress. Now the result is:
joystick 1: one axis moves S1, the other axis moves S2 & S3 at same time
joystick 2: one axis moves S3, the other axis moves S4
joystick 3: moves joystick 5 (like half speed than normal) but no rx signal in the board
record & playing are good with S1 to S4 but nor record nor playing of S5.
I think this code is very close to the solution.