Code to control robotic hand

jeffmorris1956 · October 17, 2025, 12:47pm

I built a robotic hand as shown at GitHub - pollen-robotics/AmazingHand: Code and model to control the AH!

I use Arduino Mega to control the robotic hand. I may have made a mistake when writing the code.

When I enter a number to close the fingers, the fingers should close and don’t open. How can I fix the problem?

#include <SCServo.h>
SCSCL sc;
int ID_1 = 11;
int ID_2 = 12;
int ID_3 = 13;
int ID_4 = 14;
int ID_5 = 15;
int ID_6 = 16;
int ID_7 = 17;
int ID_8 = 18;
int MiddlePos_1 = 511;
int MiddlePos_2 = 511;
int MiddlePos_3 = 511;
int MiddlePos_4 = 511;
int MiddlePos_5 = 511;
int MiddlePos_6 = 511;
int MiddlePos_7 = 511;
int MiddlePos_8 = 511;

void setup() {
  Serial1.begin(1000000);
  sc.pSerial = &Serial1;
  delay(500);
  Serial.begin(9600);
  Serial.println();
  Serial.println("1. Finger1 Open");
  Serial.println("2. Finger1 Close");
  Serial.println("3. Finger2 Open");
  Serial.println("4. Finger2 Close");
  Serial.println("5. Finger3 Open");
  Serial.println("6. Finger3 Close");
  Serial.println("7. Finger4 Open");
  Serial.println("8. Finger4 Close");
  Serial.println("9. All Fingers Open");
  Serial.println("0. All Fingers Close");

}

void loop() {
  int menuChoice = Serial.parseInt();
  Serial.println("Which Finger do you want to open or close? ");
  switch (menuChoice) {

    case 0:
      Serial.println("All Fingers are open  ");
      sc.RegWritePos(ID_1, MiddlePos_1-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_2, MiddlePos_2+300, 0, 500); //Servo 2
      sc.RegWritePos(ID_3, MiddlePos_3-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_4, MiddlePos_4+300, 0, 500); //Servo 2
      sc.RegWritePos(ID_5, MiddlePos_5-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_6, MiddlePos_6+300, 0, 500); //Servo 2
      sc.RegWritePos(ID_7, MiddlePos_7-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_8, MiddlePos_8+300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500);  
      break;

    case 1:
      Serial.println("All Fingers are closed  ");  
      sc.RegWritePos(ID_1, MiddlePos_1+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_2, MiddlePos_2-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_3, MiddlePos_3+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_4, MiddlePos_4-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_5, MiddlePos_5+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_6, MiddlePos_6-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_7, MiddlePos_7+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_8, MiddlePos_8-300, 0, 500); //Servo 2
      sc.RegWriteAction(); 
      delay(500);
      break;

    case 2:
      Serial.println("Finger1 is open  ");
      sc.RegWritePos(ID_1, MiddlePos_1-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_2, MiddlePos_2+300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500); 
      break;
    case 3:
      Serial.println("Finger1 is closed  ");
      sc.RegWritePos(ID_1, MiddlePos_1+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_2, MiddlePos_2-300, 0, 500); //Servo 2
      sc.RegWriteAction(); 
      delay(500);
      break;
    case 4:
      Serial.println("Finger2 is open  ");
      sc.RegWritePos(ID_3, MiddlePos_3-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_4, MiddlePos_4+300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500); 
      break;
    case 5:
      Serial.println("Finger2 is closed  ");
      sc.RegWritePos(ID_3, MiddlePos_3+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_4, MiddlePos_4-300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500); 
      break;
    case 6:
      Serial.println("Finger3 is open  ");
      sc.RegWritePos(ID_5, MiddlePos_5-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_6, MiddlePos_6+300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500);
      break;

    case 7:
      Serial.println("Finger3 is closed  ");
      sc.RegWritePos(ID_5, MiddlePos_5+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_6, MiddlePos_6-300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500); 
      break;

    case 8:
      Serial.println("Finger4 is open  ");
      sc.RegWritePos(ID_7, MiddlePos_7-300, 0, 500); //Servo 1
      sc.RegWritePos(ID_8, MiddlePos_8+300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500);  
      break;

    case 9:
      Serial.println("Finger4 is closed  ");
      sc.RegWritePos(ID_7, MiddlePos_7+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_8, MiddlePos_8-300, 0, 500); //Servo 2
      sc.RegWriteAction();
      delay(500); 
      break;

    //default:
    //  Serial.println("Please choose a valid selection");
  }
}

gcjr · October 17, 2025, 1:17pm

not sure without knowing what SCServo does. does it need to be initialized?

are the IDs the servo pin #s? can you test the servos using the servo library?

gcjr · October 17, 2025, 1:26pm

looking at the code again, seeing that all the MiddlePos values are the same, all the RegWritePos() values are the identical.

shouldn't they be different values depending on the desired finger position?

docdoc · October 17, 2025, 2:48pm

Yes, but I'd just add one more comment: the OP should definitely start learning how to use arrays instead of that sequence of variables! It makes the code much more compact, readable, and easily editable.

gcjr · October 17, 2025, 3:09pm

but arrays won't help without understanding that the correct values are

jeffmorris1956 · October 17, 2025, 3:13pm

The servos are controlled by serial bus servo driver board shown at Bus Servo Adapter (A) - Waveshare Wiki. I ran the code shown below and it worked.

#include <SCServo.h>
SCSCL sc;
int ID_1 = 11;
int ID_2 = 12;
int ID_3 = 13;
int ID_4 = 14;
int ID_5 = 15;
int ID_6 = 16;
int ID_7 = 17;
int ID_8 = 18;
int MiddlePos_1 = 511;
int MiddlePos_2 = 511;
int MiddlePos_3 = 511;
int MiddlePos_4 = 511;
int MiddlePos_5 = 511;
int MiddlePos_6 = 511;
int MiddlePos_7 = 511;
int MiddlePos_8 = 511;

void setup() {
  Serial1.begin(1000000);
  sc.pSerial = &Serial1;
  delay(500);
}

void loop() {
      sc.RegWritePos(ID_1, MiddlePos_1+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_2, MiddlePos_2-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_3, MiddlePos_3+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_4, MiddlePos_4-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_5, MiddlePos_5+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_6, MiddlePos_6-300, 0, 500); //Servo 2
      sc.RegWritePos(ID_7, MiddlePos_7+300, 0, 500); //Servo 1
      sc.RegWritePos(ID_8, MiddlePos_8-300, 0, 500); //Servo 2
      sc.RegWriteAction(); 
      delay(500);
}

Something may be wrong with the switch(case) loop

gcjr · October 17, 2025, 3:51pm

why not duplicate that code with different values that gets called after a 2 sec delay: delay (2000);

qubits-us · October 17, 2025, 3:58pm

maybe just start your case at 1, parseInt() returns 0 when it times out..

thinking that’s why they always open again..

good luck.. ~q

gcjr · October 17, 2025, 4:14pm

and possibly set the Serail monitor to "no line ending"

jeffmorris1956 · October 17, 2025, 7:21pm

I had to change the case numbers from 0-9 to 1-10.

What about int inByte = Serial.read(); ?

gcjr · October 17, 2025, 8:40pm

read() returns the ASCII value of the character it receives. the value of '0' is 48. the value of the linefeed char, '\n' is 10.

you can do which will translate the ASCII values of digits to integer values from 0-9'

qubits-us · October 17, 2025, 10:15pm

yes, probably better to not use parseInt at all..

maybe something like..

int GetInt() {
  static byte buff[10];
  static int inCount = 0;
  int result = -1;
  while (Serial.available()) {
    byte b = Serial.read();
    if (b != 10) {
      if (inCount < sizeof(buff)) {
        if (isdigit(b)) {
          buff[inCount] = b;
          inCount++;
        }
      } else {
        inCount = 0;
        memset(buff, 0, sizeof(buff));
      }
    } else {
      if (inCount > 0) {
        result = atoi(buff);
      }
      inCount = 0;
      memset(buff, 0, sizeof(buff));
    }
  }
  return result;
}

good luck.. ~q

gcjr · October 17, 2025, 10:48pm

qubits-us:

int GetInt() {
  static byte buff[10];
  static int inCount = 0;
  int result = -1;
  while (Serial.available()) {
    byte b = Serial.read();
    if (b != 10) {
      if (inCount < sizeof(buff)) {
        if (isdigit(b)) {
          buff[inCount] = b;
          inCount++;
        }
      } else {
        inCount = 0;
        memset(buff, 0, sizeof(buff));
      }
    } else {
      if (inCount > 0) {
        result = atoi(buff);
      }
      inCount = 0;
      memset(buff, 0, sizeof(buff));
    }
  }
  return result;
}

Serial.readBytesUntil() does that

// -----------------------------------------------------------------------------
void loop ()
{
    if (Serial.available ())  {
        char buf [90];
        int n   = Serial.readBytesUntil ('\n', buf, sizeof(buf)-1);
        buf [n] = '\0';

        Serial.println (atoi (buf));
    }
}

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

qubits-us · October 17, 2025, 11:09pm

shorter..

still see a possible timeout..

either way all good.. ~q

gcjr · October 18, 2025, 10:03am

how can there be a timeout if your looking for a specific termination char?

qubits-us · October 18, 2025, 1:14pm

that character never arrives..

hardly unlikely but possible..

~q

docdoc · October 18, 2025, 3:12pm

Sure, but, as I said, "it makes the code much more compact, readable, and easily editable."
Therefore, any kind of debugging will be easier and faster...

Topic		Replies	Views
Problem whit the code Programming	4	68	November 6, 2025
Best Way to Control Servos via Python to Arduino Interfacing w/ Software on the Computer	22	305	November 25, 2025
Problemas para programar una Mano Robotica 3d Software	4	68	February 7, 2026
Ayuda urgente por favor principiantes de arduino! Español	4	65	December 22, 2025
Can't figure out how to control servo with IR remote General Guidance	60	178	December 31, 2025

Code to control robotic hand

Related topics