Why does my robot arm's automatic mode run some steps too quickly can't be adjusted?

Using Arduino Programming Questions
1

In automatic mode, I move each servo to position A, then press "Save". Next, I move them to position B and press "Save" again. When I press "Run", they are expected to move smoothly from position A to position B. However, some of the steps are running very quickly, and I am unsure of the reason behind this behavior. It's difficult for me to describe everything through words, so I have a video for you to review and analyze.

Here is a link to my video on google drive because I can't post my video:

Here is parts list: Adruino Uno R3, HC-05, PSU 5v-10A.

Program:

#include <SoftwareSerial.h>
#include <Servo.h>

Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;
Servo servo07;
Servo servo08;

SoftwareSerial Bluetooth(3, 4); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos, servo7Pos, servo8Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos, servo7PPos, servo8PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50], servo07SP[50], servo08SP[50]; // for storing positions/steps
int speedDelay = 50;
int index = 0;
String dataIn,dataInS;

void setup() {
  Serial.begin(115200);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  servo07.attach(11);
  servo08.attach(12);
  Bluetooth.begin(9600); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(10);
  //delay(20);
  // Robot arm initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 30;
  servo3PPos =110;
  servo02.write(servo2PPos);
  servo03.write(servo3PPos);
  servo4PPos = 50;
  servo04.write(servo4PPos);
  servo5PPos = 180;
  servo05.write(servo5PPos);
  servo6PPos = 30;
  servo06.write(servo6PPos);
  servo7PPos = 90;
  servo07.write(servo7PPos);
  servo8PPos = 80;
  servo08.write(servo8PPos);
  delay(50);
}

void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.readString();  // Read the data as string
   // Serial.print(dataIn);
    // If "Base" slider has changed value - Move Servo 1 to position
    if (dataIn.startsWith("s1")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s1120" to "120"
      servo1Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          delay(15);    // 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);
          delay(15);    // defines the speed at which the servo rotates
        }
      }
      servo1PPos = servo1Pos;
    }

    //Servo2 If "Shoulder" slider has changed value - Move Servo 2 to position
    if (dataIn.startsWith("s2")) {
    String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s2140" to "140"
    servo2Pos = dataInS.toInt();  // Convert the string into integer
    // We use for loops so we can control the speed of the servo
    // If previous position is bigger then current position
    if (servo2PPos > servo2Pos) {
    for ( int j = servo2PPos; j >= servo2Pos; j--) {   // Run servo down
      servo02.write(j);
      servo03.write(140-j);
      delay(25);    // defines the speed at which the servo rotates
    }
  }
    //  If previous position is smaller then current position
    if (servo2PPos < servo2Pos) {
    for ( int j = servo2PPos; j <= servo2Pos; j++) {   // Run servo up
      servo02.write(j);
      servo03.write(140-j);
      delay(25);    // defines the speed at which the servo rotates
    }
  }
  servo2PPos = servo2Pos;
}  
    // servo4 If "Elbow" slider has changed value - Move Servo 4 to position
    if (dataIn.startsWith("s3")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s3160" to "160"
      servo4Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo4PPos > servo4Pos) {
        for ( int j = servo4PPos; j >= servo4Pos; j--) {   // Run servo down
          servo04.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo4PPos < servo4Pos) {
        for ( int j = servo4PPos; j <= servo4Pos; j++) {   // Run servo up
          servo04.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      servo4PPos = servo4Pos;
    }
    
    //servo5 If "Wrist roll" slider has changed value - Move Servo 5 to position
    if (dataIn.startsWith("s4")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s4200" to "200"
      servo5Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo5PPos > servo5Pos) {
        for ( int j = servo5PPos; j >= servo5Pos; j--) {   // Run servo down
          servo05.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo5PPos < servo5Pos) {
        for ( int j = servo5PPos; j <= servo5Pos; j++) {   // Run servo up
          servo05.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      servo5PPos = servo5Pos;
    }
    
    //servo6 If "Wist pitch" slider has changed value - Move Servo 6 to position
    if (dataIn.startsWith("s5")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s5150" to "150"
      servo6Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo6PPos > servo6Pos) {
        for ( int j = servo6PPos; j >= servo6Pos; j--) {   // Run servo down
          servo06.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo6PPos < servo6Pos) {
        for ( int j = servo6PPos; j <= servo6Pos; j++) {   // Run servo up
          servo06.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      servo6PPos = servo6Pos;
    }
    
    //Servo 7 If "Wrist pitch" slider has changed value - Move Servo 7 to position
    if (dataIn.startsWith("s6")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s6150" to "150"
      servo7Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo7PPos > servo7Pos) {
        for ( int j = servo7PPos; j >= servo7Pos; j--) {   // Run servo down
          servo07.write(j);
          delay(15);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo7PPos < servo7Pos) {
        for ( int j = servo7PPos; j <= servo7Pos; j++) {   // Run servo up
          servo07.write(j);
          delay(15);    // defines the speed at which the servo rotates
        }
      }
      servo7PPos = servo7Pos;
    }
    
    //Servo 8 If "Grip" slider has changed value - Move Servo 8 to position
    if (dataIn.startsWith("s7")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s7150" to "150"
      servo8Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo8PPos > servo8Pos) {
        for ( int j = servo8PPos; j >= servo8Pos; j--) {   // Run servo down
          servo08.write(j);
          delay(15);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo8PPos < servo8Pos) {
        for ( int j = servo8PPos; j <= servo8Pos; j++) {   // Run servo up
          servo08.write(j);
          delay(15);    // defines the speed at which the servo rotates
        }
      }
      servo8PPos = servo8Pos;
    }
// If button "SAVE" is pressed
    if (dataIn.startsWith("SAVE")) {
      servo01SP[index] = servo1PPos;  // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      servo07SP[index] = servo7PPos;
      servo08SP[index] = servo8PPos;
      index++;                        // Increase the array index
    }
     // If button "RESET" is pressed
    if ( dataIn.startsWith("RESET")) {
      memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
      memset(servo02SP, 0, sizeof(servo02SP));
      memset(servo03SP, 0, sizeof(servo03SP));
      memset(servo04SP, 0, sizeof(servo04SP));
      memset(servo05SP, 0, sizeof(servo05SP));
      memset(servo06SP, 0, sizeof(servo06SP));
      memset(servo07SP, 0, sizeof(servo07SP));
      memset(servo08SP, 0, sizeof(servo08SP));
      index = 0;  // Index to 0
    }
    // If button "RUN" is pressed----
// Automatic mode custom function - run the saved steps
   if (dataIn.startsWith("RUN")) {
  while (dataIn.startsWith("RESET")!= true) {   // 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.startsWith("PAUSE")==true) {           // If button "PAUSE" is pressed
          while (dataIn.startsWith("RUN")!=true) {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn.startsWith("RESET")== true) {     
                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);
          servo03.write(140-j);
          delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j);
          servo03.write(140-j);
          delay(speedDelay);
        }
      }

      // Servo 4
      if (servo04SP[i] == servo04SP[i + 1]) {
      }
      if (servo04SP[i] > servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      if (servo04SP[i] < servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
          servo04.write(j);
          delay(speedDelay);
        }
      }

      // Servo 5
      if (servo05SP[i] == servo05SP[i + 1]) {
      }
      if (servo05SP[i] > servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      if (servo05SP[i] < servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
          servo05.write(j);
          delay(speedDelay);
        }
      }

      // Servo 6
      if (servo06SP[i] == servo06SP[i + 1]) {
      }
      if (servo06SP[i] > servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
      if (servo06SP[i] < servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
          servo06.write(j);
          delay(speedDelay);
        }
      }

        // Servo 7
      if (servo07SP[i] == servo07SP[i + 1]) {
      }
      if (servo07SP[i] > servo07SP[i + 1]) {
        for ( int j = servo07SP[i]; j >= servo07SP[i + 1]; j--) {
          servo07.write(j);
          delay(speedDelay);
        }
      }
      if (servo07SP[i] < servo07SP[i + 1]) {
        for ( int j = servo07SP[i]; j <= servo07SP[i + 1]; j++) {
          servo07.write(j);
          delay(speedDelay);
        }
      }

        // Servo 8
      if (servo08SP[i] == servo08SP[i + 1]) {
      }
      if (servo08SP[i] > servo08SP[i + 1]) {
        for ( int j = servo08SP[i]; j >= servo08SP[i + 1]; j--) {
          servo08.write(j);
          delay(speedDelay);
        }
      }
      if (servo08SP[i] < servo08SP[i + 1]) {
        for ( int j = servo08SP[i]; j <= servo08SP[i + 1]; j++) {
          servo08.write(j);
          delay(speedDelay);
        }
      }
    }
  }
}
  }
}
2

Too much code :astonished:

3

Sorry for inconvenient.

4

You've got big blocks of code that are virtually identical.
When variable names start gaining numeric suffixes, that's when you start looking at arrays.

5

Thanks you for your suggest, I might learn to do it later but now that's not a major problem. Do you know why the servo act like that?

6

Why do you handle all servos sequentially, not all at once?

8

I don't show in the video but it happen when I run all the servo at once too, still faster at some very strange point and even run to an unsaved point very fast.

9

have you tried printing the commands on the serial monitor as they are executed?

along with speedDelay

10

No.

Why are some of the numbers odd, viz:

    // servo4 If "Elbow" slider has changed value - Move Servo 4 to position
    if (dataIn.startsWith("s3")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s3160" to "160"
      servo4Pos = dataInS.toInt();  // Convert the string into integer

You check for "s3" and use it for the servo4Pos. At the very least this is confusing. From under the umbrella here looking through the tiny window it seems wrong. Maybe it is acting weird because it is doing exactly what you wrote, not what you meant.

Part of the advantage of exploiting arrays more is that your repeated blocks of code would become each just one block. One place to find an error, one place to add an enhancemtn.


BTW, all the lines that look like this

      if (servo02SP[i] == servo02SP[i + 1]) {
      }

don't do anything and can be removed.

"If they are equal, do nothing. Otherwise do nothing."

a7

11

yes, they look normal and speedDelay can actually change the speed but not the part where they run very fast.

12

I will think about what you said and try to finger out what's wrong.

13

Let the IDE format your code CTRL-T so that you can find out possibly wrong brace structure.

14

Thank you

15

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.