Servos not returning to original position

[dominicklee]

Hello, I have the Arduino Bracio++ kit and have a fully assembled robot. The board I am using is the included Arduino Braccio Carrier, which has the Nano RP2040.

I have tried most if not all of the examples in this library and started to write some code of my own, but encountered a possible bug where the servos are very weak after they are "disengaged" and subsequently "engaged" again.

The objective of my code is simply to do the following:

• Upon start, set the servos at the factory angles (straightened robot)
• Release the motors (so the user can physically change the pose)
• When the user pressed the "Enter" button, get the positions of all the servos, then print them out
• Wait a couple seconds, engage motors, and then return all the servos back to the factory angles again
• Release the motors (so the user can physically change the pose)

The problem is that if I physically change one servo, when I press the "Enter" button, the servos revert to factory angles, but if I change multiple (anywhere between 2-6) servos, then I encounter issues where one or more servos will not return back to the factory angles. Pressing the "Enter" button again will cause those servos to very slowly move, but nowhere near the expected speed at startup.

My theory is that something is happening to the power chip or servos after they are "disengaged", and the "engage" method does not restore it the same way that a hard power cycle would. Please help resolve. Ideally, we don't want to power cycle the board when such a glitch occurs.

This is my full code:

#include <Braccio++.h>

#define BUTTON_ENTER   6

// Braccio ++ joints
auto gripper    = Braccio.get(1);
auto wristRoll  = Braccio.get(2);
auto wristPitch = Braccio.get(3);
auto elbow      = Braccio.get(4);
auto shoulder   = Braccio.get(5);
auto base       = Braccio.get(6);

/* Variables */
float servoAngles[6] = {gripper, wristRoll, wristPitch, elbow, shoulder, base};
static float const homePos[6] = {157.50, 157.50, 157.50, 157.50, 157.50, 90.00};

bool btnPressed = false; // Flag to indicate button pressed

void fetchAngles() {
  // Update servoAngles with real angles
  for (int i = 0; i < 6; i++) { // Iterate motor angles
    servoAngles[i] = Braccio.get(i).position();
  }
}

void setSpeeds() {
  Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
  Braccio.speed(gripper, MEDIUM);
  Braccio.speed(wristRoll, MEDIUM);
  delay(100);
}

void setup() {
  Braccio.begin();
  delay(500); // Waits for the Braccio initialization

  setSpeeds();

  // Set motor initial angles. Should move all the motors at once
  Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
  delay(500);
  Braccio.disengage();
  delay(100);
  Serial.println("Move robot to desired position.");
  Serial.println("Press Enter button to display angles and return to home position.");
}

void loop() {
  int pressedKey = Braccio.getKey();

  if (pressedKey == BUTTON_ENTER)
    btnPressed = true; // Trigger joints' movements

  if (btnPressed) {
    fetchAngles();
    Serial.println("Current Motor angles [M 1-6]");
    for (int i = 0; i < 6; i++) { // Iterate motor angles
      Serial.print(servoAngles[i]); 
      if (i != 5)
        Serial.print(",");
    }
    Serial.println();
    delay(100);
    Serial.println("Stand clear. Returning to initial angles");
    delay(2000);
    Braccio.engage(); delay(500);
    setSpeeds();
    for (int i = 0; i < 3; i++) {
      Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
      delay(2000);
    }
    Braccio.disengage(); delay(500);

    btnPressed = false; // Stop joints' movements
  }

}

[dominicklee]

I've actually solved it by simply git cloning directly from this repository. The latest code works nicely. However, the bug exists on the "official release" on Arduino library manager (v1.3.2). Would someone please roll out the latest version of this repo as v1.3.3? Thanks in advance.

Here is my slightly revised code which works now:

#include <Braccio++.h>

#define BUTTON_ENTER   6

// Braccio ++ joints
auto gripper    = Braccio.get(1);
auto wristRoll  = Braccio.get(2);
auto wristPitch = Braccio.get(3);
auto elbow      = Braccio.get(4);
auto shoulder   = Braccio.get(5);
auto base       = Braccio.get(6);

/* Variables */
float servoAngles[6] = {gripper, wristRoll, wristPitch, elbow, shoulder, base};
static float const homePos[6] = {157.50, 157.50, 157.50, 157.50, 157.50, 90.00};

bool btnPressed = false; // Flag to indicate button pressed

void fetchAngles() {
  // Update servoAngles with real angles
  for (int i = 0; i < 6; i++) { // Iterate motor angles
    servoAngles[i] = Braccio.get(i).position();
  }
}

void setup() {
  Braccio.begin();
  delay(500); // Waits for the Braccio initialization

  Braccio.setAngularVelocity(25.0f);
  delay(100);

  // Set motor initial angles. Should move all the motors at once
  Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
  delay(500);
  Braccio.disengage();
  delay(100);
  Serial.println("Move robot to desired position.");
  Serial.println("Press Enter button to display angles and return to home position.");
}

void loop() {
  int pressedKey = Braccio.getKey();

  if (pressedKey == BUTTON_ENTER)
    btnPressed = true; // Trigger joints' movements

  if (btnPressed) {
    fetchAngles();
    Serial.println("Current Motor angles [M 1-6]");
    for (int i = 0; i < 6; i++) { // Iterate motor angles
      Serial.print(servoAngles[i]); 
      if (i != 5)
        Serial.print(",");
    }
    Serial.println();
    delay(100);
    Serial.println("Stand clear. Returning to initial angles");
    delay(2000);
    Braccio.engage(); delay(500);
    for (int i = 0; i < 3; i++) {
      Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
      delay(2000);
    }
    Braccio.disengage(); delay(500);

    btnPressed = false; // Stop joints' movements
  }

}