Repeating servo functions (program modification)

Hi, can someone help me with program?
Arduino Uno board.
I want my servos to work at the same time. Now they work one after another.
Im trying to use like Left and Right servos: servo1 and servo2
So how to modifie this code:

#include <SoftwareSerial.h>
#include <Servo.h>
Servo servo01;
Servo servo02;

SoftwareSerial Bluetooth(10, 11); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
int servo1Pos, servo2Pos; // current position
int servo1PPos, servo2PPos; // previous position
int servo01SP[50], servo02SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
int b;
int c;
String dataIn = "";
void setup() {
  servo01.attach(4);
  servo02.attach(5);
  Serial.begin(9600);
   
  Bluetooth.begin(9600); // Default baud rate of the Bluetooth module
  
  delay(20);
  // Robot arm initial position
  servo1PPos = 84;
  servo2PPos = 162 - servo1PPos;
  servo01.write(servo1PPos); 
  servo02.write(servo2PPos);   

}
void loop() {
  // Check for incoming data
  if (Bluetooth.available() >= 1) {
    dataIn = Bluetooth.readString();  // Read the data as string

    
    // If "Waist" slider has changed value - Move Servo 1 to position
    if (dataIn.startsWith("s6")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo1Pos = dataInS.toInt();  
     if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          servo02.write(162 - j);                 
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo1PPos < servo1Pos) {
        for ( int j = servo1PPos; j <= servo1Pos; j++) {   // Run servo up
          servo01.write(j);
          servo02.write(162 - j);                   
          delay(20);
        }
      }
      servo1PPos = servo1Pos; 
         
         // set current position as previous position
    }
    




    // If button "SAVE" is pressed
    if (dataIn.startsWith("SAVE")) {
       servo01SP[index] = servo1PPos;  // save position into the array
       servo02SP[index] = servo2PPos;
          index++;                        // Increase the array index
    }
    // If button "RUN" is pressed
    if (dataIn.startsWith("RUN")) {
      runservo();  // Automatic mode - run the saved steps 
    }
    // If button "RESET" is pressed
    if ( dataIn == "RESET") {
      memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
      memset(servo02SP, 0, sizeof(servo02SP));
    
      index = 0;  // Index to 0
    }
  }
}
// Automatic mode custom function - run the saved steps
void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {     
                break;
              }
            }
          }
        }        
      }      
    }
  }
}

Post the whole program.

Post the whole program in code tags.

Read the forum guidelines.

Which Arduino board?

See how to make motor move in a period of time without using delay

SoftwareSerial.h and Servo.h have problems with compatibility. Try replacing SoftwareSerial with NeoSWSerial.h which might help.

Steve

Thanks Steve,
It helped with servo moving smoother. But the problem I can't figure out how to make save function to work.

UPDATE:
Still need help with the code:
Its working, but as I mentioned before. I need code configuration:
In video it shows after launching saved possition’s run mode. When they move manual slider they move together.
The problem video: https://youtu.be/cZ2eWf2wCdk
I want them to work together. Not 1 by 1.
I think the problem is here:

 if (dataIn.startsWith("SAVE")) {
       servo01SP[index] = servo1PPos;  // save position into the array
       servo02SP[index] = servo2PPos;
          index++;                        // Increase the array index
    }

Full code:

#include <NeoSWSerial.h> // PRIES TAI BUVES: SoftwareSerial.h
#include <Servo.h>
Servo servo01;
Servo servo02;

NeoSWSerial Bluetooth(10, 11); // PRIES TAI BUVES: SoftwareSerial.h
int servo1Pos, servo2Pos; 
int servo1PPos, servo2PPos; 
int servo01SP[50], servo02SP[50]; 
int speedDelay = 20;
int index = 0;
String dataIn = "";
void setup() {
  servo01.attach(4);
  servo02.attach(5);    
  Bluetooth.begin(9600); 
  Bluetooth.setTimeout(5);
  delay(20);
  // Robot arm initial position
  servo1PPos = 84;  
  servo01.write(servo1PPos); 
  servo2PPos = 162 - servo1PPos;
  servo02.write(servo2PPos);   
}
void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.readString();  
        
    if (dataIn.startsWith("s6")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo1Pos = dataInS.toInt();  
     if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          servo02.write(162 - j);                 
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo1PPos < servo1Pos) {
        for ( int j = servo1PPos; j <= servo1Pos; j++) {   // Run servo up
          servo01.write(j);
          servo02.write(162 - j);                   
          delay(20);
        }
      }
      servo1PPos = servo1Pos;  
      servo2PPos = 162 - servo1Pos;    
         // set current position as previous position
    }
     
    if (dataIn.startsWith("SAVE")) {
       servo01SP[index] = servo1PPos;  // save position into the array
       servo02SP[index] = servo2PPos;
          index++;                        // Increase the array index
    }
    
    if (dataIn.startsWith("RUN")) {
      runservo();  // Automatic mode - run the saved steps 
    }
    // If button "RESET" is pressed
    if ( dataIn == "RESET") {
      memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
      memset(servo02SP, 0, sizeof(servo02SP));    
      index = 0;  // Index to 0
    }
  }
}
// Automatic mode custom function - run the saved steps
void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {     
                break;
              }
            }
          }
        }
        // If speed slider is changed
        if (dataIn.startsWith("ss")) {
          String dataInS = dataIn.substring(2, dataIn.length());
          speedDelay = dataInS.toInt(); // Change servo speed (delay time)
        }
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) {
      }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      // Servo 2
      if (servo02SP[i] == servo02SP[i + 1]) {
      }
      if (servo02SP[i] > servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j);
          delay(speedDelay);
        }        
      }      
    }
  }
}

I'm not really clear what the problem is. But it may be something to do with the for loops in runServo(). You complete a move of servo1 before you even start checking if servo2 needs to move so obviously they will move one after the other.

But since they can presumably have completely different positions saved for a particular array index I don't see what "moving together" means when they may be moving from 2 different positions to 2 other different positions. Can you give some examples of what values you are putting into the two ...SP arrays and what exactly you expect to happen.

Steve

Yes Steve, you are completely right. The problem is in this for loops in runServo().
Servo1 is the leading servo. Servo 2 is just following it. Thats what I want from this situation.
So please, can you help me to solve it?

void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {    
                break;
              }
            }
          }
        }

Maybe you can also explain what “index - 2” is for?

UPDATED VIDEO!

Hi,

For more understanding I give longer video. Sorry about the delay.

Regards

So you didn't write that code? And you don't understand how it works? That does make things difficult.

From the changes you've made so far I think you want the servos always to move according to the rule (whenever servo1 goes to position x, servo2 goes to position 162-x). Is that correct? If that is all you need then the save and run routines are way too complicated for what they need to do. It would probably be easier to start again from scratch.

If that's not what you need then explain IN DETAIL exactly what you are trying to do.

Steve

Steve, its totaly not compicated. I have solved it myself with 2 short code lines at speed slider changing. But still thanks for your precious time.

-THIS CASE IS CLOSED-

How is it closed?
I don’t see the solution.

Solution:
int a = 162;
Removed speed slider servo 2 and everything works copying servo 1.
Put this code in right places, like speed slider and so on:
servo02.write(a - j);
And now it works totally fine.

 #include <NeoSWSerial.h> // before: SoftwareSerial.h
#include <Servo.h>
Servo servo01;
Servo servo02;

NeoSWSerial Bluetooth(10, 11); // before: SoftwareSerial.h
int servo1Pos, servo2Pos; 
int servo1PPos, servo2PPos; 
int servo01SP[50], servo02SP[50]; 
int speedDelay = 20;
int a = 162;
int index = 0;
String dataIn = "";
void setup() {
  servo01.attach(4);
  servo02.attach(5);      
  Bluetooth.begin(9600); 
  Bluetooth.setTimeout(5);  
  delay(20);
  // Robot arm initial position
  servo1PPos = 84;  
  servo01.write(servo1PPos); 
  servo2PPos = a - servo1PPos;
  servo02.write(servo2PPos);   
}
void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.readString();  
        
    if (dataIn.startsWith("s6")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo1Pos = dataInS.toInt();  
     if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          servo02.write(a - j);                 
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo1PPos < servo1Pos) {
        for ( int j = servo1PPos; j <= servo1Pos; j++) {   // Run servo up
          servo01.write(j);
          servo02.write(a - j);                   
          delay(20);
        }
      }
      servo1PPos = servo1Pos;  
      servo2PPos = a - servo1Pos; 
               
    }
     
    if (dataIn.startsWith("SAVE")) {
       servo01SP[index] = servo1PPos; 
       servo02SP[index] = servo2PPos; 
          index++;                       
    }
    
    if (dataIn.startsWith("RUN")) {
      runservo();  // Automatic mode - run the saved steps 
    }
    // If button "RESET" is pressed
    if ( dataIn == "RESET") {
      memset(servo01SP, 0, sizeof(servo01SP));
      memset(servo02SP, 0, sizeof(servo02SP));// Clear the array data to 0          
      index = 0;  // Index to 0
    }
  }
}
// Automatic mode custom function - run the saved steps
void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {     
                break;
              }
            }
          }
        }
        // If speed slider is changed
        if (dataIn.startsWith("ss")) {
          String dataInS = dataIn.substring(2, dataIn.length());
          speedDelay = dataInS.toInt(); // Change servo speed (delay time)
        }
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) {
      }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          servo02.write(a - j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          servo02.write(a - j);
          delay(speedDelay);
        }           
      }      
    }
  }
}

I’m glad you have it working and now finally I can tell what it is that you were trying to get it to do.

You could save some memory and some complication by getting rid of the servo02SP array completely. There’s no point having the save routine write things into it when you never read them back or use them in the run routine.

Steve

Ok. Thanks for good advice.