Background information: I am working to control a 6 servo robot arm (Tinkerkit Braccio robot, Tinkerkit Braccio robot — Arduino Official Store) using serial communication to an Arduino Uno R3. The serial communication is received by the Arduino through the hardware serial/USB plug and processed using the code given below,
#include <Servo.h>
#include <Braccio.h>
#include <NeoSWSerial.h>
#define SERVO_BASE_PIN 11
#define SERVO_SHOULDER_PIN 10
#define SERVO_ELBOW_PIN 9
#define SERVO_WRIST_VER_PIN 6
#define SERVO_WRIST_ROT_PIN 5
#define SERVO_GRIPPER_PIN 3
#define BASE_START 0
#define SHOULDER_START 90 //Shoulder servo angles are currently off from what they should be.
// Need to rotate the servo plate 45 degrees to left (when looking at the plate)
#define ELBOW_START 90
#define WRIST_VER_START 90
#define WRIST_ROT_START 90
#define GRIPPER_START 30
NeoSWSerial mySerial(2,4); // RX = 2, TX = 4
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;
uint8_t idx = 0;
uint8_t value_idx = 0;
char value[4] = "";
//The problem is in this if and for loop. Something is causing the arduino to add an additional zero to the pos counter.
void reach_goal(Servo& motor, String motorN, int goal){
if(goal>=motor.read()){
for(int pos = motor.read(); pos <= goal; pos += 1){
mySerial.print("Current goal: ");
mySerial.println(goal);
mySerial.print("Current position: ");
mySerial.println(pos);
mySerial.println("Servo used: " + motorN);
motor.write(pos);
delay(5);
}
}
else{
for(int pos = motor.read(); pos >= goal; pos -= 1){
//mySerial.print("Current goal: ");
//mySerial.println(goal);
//mySerial.print("Current position: ");
//mySerial.println(pos);
//mySerial.println("Servo used:" + motorN);
motor.write(pos);
delay(5);
}
}
}
void setup() {
// put your setup code here, to run once:
Braccio.begin();
base.attach(SERVO_BASE_PIN);
shoulder.attach(SERVO_SHOULDER_PIN);
elbow.attach(SERVO_ELBOW_PIN);
wrist_ver.attach(SERVO_WRIST_VER_PIN);
wrist_rot.attach(SERVO_WRIST_ROT_PIN);
gripper.attach(SERVO_GRIPPER_PIN);
base.write(BASE_START);
shoulder.write(SHOULDER_START);
elbow.write(ELBOW_START);
wrist_ver.write(WRIST_VER_START);
wrist_rot.write(WRIST_ROT_START);
gripper.write(GRIPPER_START);
Serial.begin(115200);
Serial.setTimeout(1);
//Serial.println("Initial value:");
//Serial.print(value);
//mySerial.begin(9600);
//delay(1000);
//mySerial.println("Connection to Elegoo Arduino initiated");
}
void loop() {
// put your main code here, to run repeatedly:
while(Serial.available() > 0){
//mySerial.println("Reading serial data");
char chr = Serial.read();
//Serial.println(chr);
if(chr == 'b'){
idx = 0;
value_idx = 0;
}
else if (chr == 's'){
idx = 1;
value_idx = 0;
}
else if (chr == 'e'){
idx = 2;
value_idx = 0;
}
else if (chr == 'r'){
idx = 3;
value_idx = 0;
}
else if (chr == 'v'){
idx = 4;
value_idx = 0;
}
else if (chr == 'g'){
idx = 5;
value_idx = 0;
}
else if (chr == ','){
//Serial.print("Initial value before atoi");
//Serial.println(value);
int val = strtol(value, NULL, 0);
if(idx == 0){
//Serial.print("value0: ");
//Serial.println(val);
reach_goal(base, "base", val);
}
else if (idx == 1){
//Serial.print("value1: ");
//Serial.println(val);
reach_goal(shoulder, "shoulder", val);
}
else if (idx == 2){
//Serial.print("value2: ");
//Serial.println(val);
reach_goal(elbow, "elbow", val);
}
else if (idx == 3){
//Serial.print("value3: ");
//Serial.println(val);
reach_goal(wrist_rot, "wrist_rot", val);
}
else if (idx == 4){
//Serial.print("value4: ");
//Serial.println(val);
reach_goal(wrist_ver, "wrist_ver", val);
}
else if (idx == 5){
//Serial.print("value5: ");
//Serial.println(val);
reach_goal(gripper, "gripper", val);
}
value[0] = 0;
value[1] = 0;
value[2] = 0;
value[3] = '\0"';
}
else{
value[value_idx] = chr;
value_idx++;
//String current_pos = String(base.read()) + "," + String(shoulder.read()) + "," + String(elbow.read()) + "," + String(wrist_ver.read()) + "," + String(wrist_rot.read()) + "," + String(gripper.read());
//mySerial.println("The current position of each servo is (in order):" + current_pos);
//delay(1000);
}
}
}
The data being sent is the wanted angle positions for each of the servos, with a corresponding letter identifier (Ex. b90, s90, e90, v90, r90, g20). This data is generated and sent by Robot Operating System 2 (ROS2, Humble distribution) and MoveIt2 on an Ubuntu 22.04 system. The system operates by being given wanted servo/joint positions and having MoveIt2 generate a movement pathing plan. This plan is then sent to the Arduino in step increments, using the following code and a baud rate of 115200. I have included the entire body of code, but the main function is write.
#include <braccio_arm_config/braccio_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_list_macros.hpp>
#include <chrono>
namespace braccio_arm_config{
BraccioInterface::BraccioInterface(){
}
BraccioInterface::~BraccioInterface(){
if(arduino_.IsOpen()){
try{
arduino_.Close();
}
catch(...){
RCLCPP_FATAL_STREAM(rclcpp::get_logger("BraccioInterface"), "Something has gone wrong while closing the connection with the port " << port_);
}
}
}
CallbackReturn BraccioInterface::on_init(const hardware_interface::HardwareInfo & hardware_info){
CallbackReturn result = hardware_interface::SystemInterface::on_init(hardware_info);
if(result != CallbackReturn::SUCCESS){
return result;
}
try{
port_ = info_.hardware_parameters.at("port");
}
catch(const std::out_of_range &e){
RCLCPP_FATAL_STREAM(rclcpp::get_logger("BraccioInterface"), "No serial port provided. Aborting.");
return CallbackReturn::FAILURE;
}
position_commands_.reserve(info_.joints.size());
position_states_.reserve(info_.joints.size());
prev_position_commands_.reserve(info_.joints.size());
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> BraccioInterface::export_state_interfaces(){
std::vector<hardware_interface::StateInterface> state_interfaces;
for(size_t i = 0; i < info_.joints.size(); i++){
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_states_[i]));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> BraccioInterface::export_command_interfaces(){
std::vector<hardware_interface::CommandInterface> command_interfaces;
for(size_t i = 0; i < info_.joints.size(); i++){
command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_commands_[i]));
}
return command_interfaces;
}
CallbackReturn BraccioInterface::on_activate(const rclcpp_lifecycle::State & previous_state){
RCLCPP_INFO(rclcpp::get_logger("BraccioInterface"), "Starting the Braccio robot hardware.");
//angles go in order base, shoulder, elbow, wrist_ver, wrist_rot, grippers
position_commands_ = {0.0, -0.506, -0.611, -1.100, 0.0, 0.0}; //Need to adjust these starting positions.
prev_position_commands_ = {0.0, -0.506, -0.611, -1.100, 0.0, 0.0};
position_states_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
try{
arduino_.Open(port_);
arduino_.SetBaudRate(LibSerial::BaudRate::BAUD_115200);
}
catch(...){
RCLCPP_FATAL_STREAM(rclcpp::get_logger("BraccioInterface"), "Something went wrong while interacting with the serial port " << port_);
return CallbackReturn::FAILURE;
}
RCLCPP_INFO(rclcpp::get_logger("BraccioInterface"), "The Braccio arm has started. Ready to receive position input.");
return CallbackReturn::SUCCESS;
}
CallbackReturn BraccioInterface::on_deactivate(const rclcpp_lifecycle::State & previous_state){
RCLCPP_INFO(rclcpp::get_logger("BraccioInterface"), "Stopping the Braccio arm.");
if(arduino_.IsOpen()){
try{
arduino_.Close();
}
catch(...){
RCLCPP_FATAL_STREAM(rclcpp::get_logger("BraccioInterface"), "Something went wrong while disconnecting from the serial port " << port_);
return CallbackReturn::FAILURE;
}
}
RCLCPP_INFO(rclcpp::get_logger("BraccioInterface"), "The Braccio arm has stopped.");
return CallbackReturn::SUCCESS;
}
hardware_interface::return_type BraccioInterface::read(const rclcpp::Time & time, const rclcpp::Duration & period){
//Due to having dumb servos we will assume the servos reached the requested position. ***Not inherently true***.
//position_states_ = arduino_.Read();
//std::string return_msg;
position_states_ = position_commands_;
return hardware_interface::return_type::OK;
}
hardware_interface::return_type BraccioInterface::write(const rclcpp::Time & time, const rclcpp::Duration & period){
if(position_commands_ == prev_position_commands_){
return hardware_interface::return_type::OK;
}
//b## = base<angle>, s## = shoulder<angle>, e## = elbow<angle>, v## = Wrist_ver<angle>, r## = Wrist_rot<angle>, g## = gripper<angle>
std::string msg;
std::string msg_rad;// The equations given in the lecture are provided by the UDEMY course instructor. I do not believe they apply to the Braccio arm so they are ommitted.
//The following equations are converting from radians to degrees. As ros2_control uses radians.
//Shifts are applied to each joint angle output due to the angle limits in Moveit/RViz differing from the real hardware.
//This is due to the zero position of the urdf model and the real hardware being different.
int base = static_cast<int>((position_commands_.at(0) * 180) / M_PI);
double base_rad = static_cast<double>(position_commands_.at(0));
msg.append("b");
msg.append(std::to_string(base));
msg.append(",");
msg_rad.append("b");
msg_rad.append(std::to_string(base_rad));
msg_rad.append(",");
int shoulder = static_cast<int>(((position_commands_.at(1)*180)/M_PI)+119);
double shoulder_rad = static_cast<double>(position_commands_.at(1));
msg.append("s");
msg.append(std::to_string(shoulder));
msg.append(",");
msg_rad.append("s");
msg_rad.append(std::to_string(shoulder_rad));
msg_rad.append(",");
int elbow = static_cast<int>(((position_commands_.at(2)*180)/M_PI)+125);
double elbow_rad = static_cast<double>(position_commands_.at(2));
msg.append("e");
msg.append(std::to_string(elbow));
msg.append(",");
msg_rad.append("e");
msg_rad.append(std::to_string(elbow_rad));
msg_rad.append(",");
int wrist_ver = static_cast<int>(((position_commands_.at(3)*180)/M_PI)+153);
double wrist_ver_rad = static_cast<double>(position_commands_.at(3));
msg.append("v");
msg.append(std::to_string(wrist_ver));
msg.append(",");
msg_rad.append("v");
msg_rad.append(std::to_string(wrist_ver_rad));
msg_rad.append(",");
int wrist_rot = static_cast<int>((position_commands_.at(4)*180)/M_PI);
double wrist_rot_rad = static_cast<double>(position_commands_.at(4));
msg.append("r");
msg.append(std::to_string(wrist_rot));
msg.append(",");
msg_rad.append("r");
msg_rad.append(std::to_string(wrist_rot_rad));
msg_rad.append(",");
int grippers = static_cast<int>(((position_commands_.at(5)*180)/M_PI)+28);
double grippers_rad = static_cast<double>(position_commands_.at(5));
msg.append("g");
msg.append(std::to_string(grippers));
msg.append(",");
msg_rad.append("g");
msg_rad.append(std::to_string(grippers_rad));
msg_rad.append(",");
try{
RCLCPP_INFO_STREAM(rclcpp::get_logger("BraccioInterface"), "Attempting to send the following message to the Arduino (in degrees): " << msg);
RCLCPP_INFO_STREAM(rclcpp::get_logger("BraccioInterface"), "Attempting to send the following message to the Arduino (in radians): " << msg_rad);
arduino_.Write(msg);
arduino_.DrainWriteBuffer();
std::chrono::milliseconds timespan(1000);
}
catch(const std::runtime_error& re)
{
// specific handling for runtime_error
RCLCPP_ERROR_STREAM(rclcpp::get_logger("BraccioInterface"), "Runtime error: " << re.what() << std::endl);
}
catch(const std::exception& ex)
{
// specific handling for all exceptions extending std::exception, except
// std::runtime_error which is handled explicitly
RCLCPP_ERROR_STREAM(rclcpp::get_logger("BraccioInterface"), "Error occurred: " << ex.what() << std::endl);
}
catch(...){
RCLCPP_ERROR_STREAM(rclcpp::get_logger("BraccioInterface"), "Something has gone wrong while sending the message to the arduino. " << msg);
RCLCPP_ERROR_STREAM(rclcpp::get_logger("BraccioInterface"), "The exact error is unknown.");
return hardware_interface::return_type::ERROR;
}
prev_position_commands_ = position_commands_;
return hardware_interface::return_type::OK;
}
}
PLUGINLIB_EXPORT_CLASS(braccio_arm_config::BraccioInterface, hardware_interface::SystemInterface);
The wanted positions can either be generated through the robot visualization software or through a simple Python GUI which I had made.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from braccio_msgs.action import BraccioTask
import time
import PySimpleGUI as sg
#Defining the button event for each position button.
def position0(self):
self.goal.task_number = 0
self.get_logger().info("Moving Braccio arm to the safety position. Please wait until movement stops.")
def position1(self):
self.goal.task_number = 1
self.get_logger().info("Moving Braccio arm to the pickup position with grippers open. Please wait until movement stops.")
def position2(self):
self.goal.task_number = 2
self.get_logger().info("Moving Braccio arm to the pickup position with grippers closed. Please wait until movement stops.")
def position3(self):
self.goal.task_number = 3
self.get_logger().info("Moving Braccio arm to its resting position. Please wait until movement stops.")
def positionEnd(self):
self.get_logger().info("Moving the Braccio arm to the safety position and shutting down program.")
self.goal.task_number = 0
self.action_client.send_goal_async(self.goal)
time.sleep(10)
self.destroy_node()
rclpy.shutdown()
class PythonInterface(Node):
def __init__(self):
super().__init__("python_interface")
self.action_client = ActionClient(Node("python_interface"), BraccioTask, "task_server")
self.action_client.wait_for_server()
self.goal = BraccioTask.Goal()
self.get_logger().info("Python interface initialized.")
dispatch_dictionary = {'0 - Safety position':position0, '1 - Pickup position, grippers open':position1, '2 - Pickup position, grippers closed':position2, '3 - Rest position':position3, '* - Exit and shutdown':positionEnd}
sg.theme('Default')
layout = [[sg.Text("Please click the button corresponding to the desired position. The positions are as follows:")],
[sg.Button('0 - Safety position')],
[sg.Button('1 - Pickup position, grippers open')],
[sg.Button('2 - Pickup position, grippers closed')],
[sg.Button('3 - Rest position')],
[sg.Button('* - Exit and shutdown')],
[sg.Text('You may also input custom angle positions (degrees) in the format: b##,s##,e##, v##, r##, g## (I.e. b0,s90,e90,v120,r20,g10)')],
[sg.InputText(focus=True, key='-IN-')]
]
window = sg.Window('Braccio Arm Control Interface', layout, finalize=True)
window['-IN-'].bind("<Return>", "_Enter")
while True:
event, values = window.read()
if event == sg.WIN_CLOSED:
break #The exit window button was clicked. The GUI window will close and shutdown.
if event in dispatch_dictionary: #If a button for a predefined position is clicked then we check which one and set the task_number to the respective value for the position.
called_movement = dispatch_dictionary[event]
called_movement(self)
self.goal.position_angles = "empty"
elif event == "-IN-" + "_Enter": #If a custom set of inputs were given then we input them into a string and pass them to the task server.
self.goal.position_angles = values['-IN-']
self.goal.task_number = 0
self.action_client.send_goal_async(self.goal) #Sends the goal to the task server.
def main():
rclpy.init()
action_client = PythonInterface()
rclpy.spin(action_client)
if __name__ == '__main__':
main()
(Sorry if the previous section was quite long. I do not have a background in robotics so I want to be thorough in the explanation of how I set things up in case I made a rookie mistake)
Problems: The servos act erratically when rapid sets of serial data is sent from the computer to the Arduino. The servos will typically behave well if individual servo positions are changed or if the serial communication is sent using the Python GUI.
However, if positions are sent through the robot visualization interface, or too rapidly, 5 of the servos (base doesn't) will begin to move from their minimum positions to their maximum positions in order of the servos (shoulder, elbow, wrist vertical, wrist rotation, grippers). The servos then proceed to their desired positions afterwards. I have included a video to show this behaviour.
This problem becomes present when using the Python GUI after it starts, but I haven't seen it start from using the Python GUI.
Troubleshooting: I have been attempting to troubleshoot this for a couple weeks. Similar topics I have read suggest power issues, serial buffer overflow, or out of array bounds.
Power issues - I haven't seen direct signs on power issues such as the Arduino resetting. There is a set up function for the Braccio arm which initializes the servos and moves them to a set position. I do not see the arm move back to its preset position so I don't believe it is resetting due to insufficient power. Though this may be incorrect.
Serial buffer overflow - I have attempted prevent the risk of a serial buffer overflow by implementing a delay time for the sending of serial data on the computer side. I have also used the Libserial DrainWriteBuffer() function to wait until the previous serial data has been sent.
Out of array bounds - I have tried to look for anywhere in the Arduino code which could result in an out of array bounds error but I haven't found one that sticks out. My knowledge of the different functions used is not perfect though.
Secondary Problem: I have tried to troubleshoot what is being written to the servos on the Arduino end but I cannot use the Serial Monitor while the computer is connected to the serial port. Instead I have been trying to use a second Arduino and the NeoSWSerial library to pass the information to the computer. This results in the robot arm shaking when serial data is read and the serial data is not fully sent to the second Arduino. I believe this is due to the Servo library conflicting with NeoSWSerial due to interrupts but I haven't found a way to properly fix this.
Thank you to those who made it this far and I appreciate any advice that can be given.
TL;DR Five of the servos (excluding base) in a 6 servo robot arm begin moving to their minimum position then maximum position, one at a time, before moving to the requested positions. This seems to happen when the Arduino controlling the servos receives serial data rapidly.
