Arduino doesnt work after a single tested function gets compiled in new function

sedihglow · November 11, 2021, 7:04pm

Hello everyone,

I am running into an issue with Arduino i have not ran into yet. Everything has been running smoothly until i added a new function called send_all_angles() that calls two other functions in its class. It does not reach this function in the program before the effect is seen. After seeing this i commented out the two function calls in send_all_angles() and it worked fine again. After that i uncommented the first function, it worked, and anytime i comment out the second function the error occurs. Compiler has no warnings and the function call that causes the error in send_all_angles() is tested and working in other functions.

This is running on an Arduino UNO and is operating a Braccio Robot Arm. When the code in uncommented the robot arm becomes unresponsive (it does not even go into its default position). This is all before send_all_angles() is called.

The error is it either resets over and over or sends garbage on the serial.

With the function create_send_msg() in send_all_angles() commented out (which is the line that causes the error) the compiler says

Sketch uses 8330 bytes (25%) of program storage space. Maximum is 32256 bytes.
Global variables use 797 bytes (38%) of dynamic memory, leaving 1251 bytes for local variables. Maximum is 2048 bytes.

With the function put into the program the compiler says

Sketch uses 8608 bytes (26%) of program storage space. Maximum is 32256 bytes.
Global variables use 831 bytes (40%) of dynamic memory, leaving 1217 bytes for local variables. Maximum is 2048 bytes.

send_all_angles() can be found on line 82 of braccio_arm.cpp and is the 4th or 5th function down.

braccio_arm.h below

#ifndef _BRACCIO_ARM_H
#define _BRACCIO_ARM_H

#include <Arduino.h>
#include <stdint.h>
#include <errno.h>
#include "braccio.h"
#include <Servo.h>

#define DFLT_STEP_DELAY 20 // ms

#define MSG_SIZE_NO_PARAM 4 // msg_len, msg_type, cmd, param_len, 4 bytes
#define PARAM_BUFF 250
#define IO_MSG_BUFF (MSG_SIZE_NO_PARAM + PARAM_BUFF)

#define PARAM_STRLEN 0

// msg type definitions
#define CMD_MSG 0x0
#define PRINT_MSG 0x1
#define ACK 0x2
#define FINISH 0x3

// outgoing cmd definitions
#define PRINT_GENERAL 0x0
#define PRINT_ERROR 0x1
#define PRINT_VERBOSE 0x2
#define SEND_ANGLES 0x3

// incomming cammands
enum servo_angle_cmd {
    M1_ANGLE=1, // base 1
    M2_ANGLE,   // shoulder 2
    M3_ANGLE,   // elbow 3
    M4_ANGLE,   // wrist vertial 4
    M5_ANGLE,   // wrist rotation 5
    M6_ANGLE,   // gripper 6
    MX_ANGLE,    // braccio_arm for all servo angles at once 7
    REQUEST_MX_ANGLE // requesting all angles to be sent 8
};

// min max angles
#define M1_MIN_ANGLE 0
#define M1_MAX_ANGLE 180
#define M2_MIN_ANGLE 15
#define M2_MAX_ANGLE 165
#define M3_MIN_ANGLE 0
#define M3_MAX_ANGLE 180
#define M4_MIN_ANGLE 0
#define M4_MAX_ANGLE 180
#define M5_MIN_ANGLE 0
#define M5_MAX_ANGLE 180
#define M6_MIN_ANGLE 10 // tongue is open
#define M6_MAX_ANGLE 73 // gripper is closed

// Safety angles
// NOTE: This may differ from the getting started guide. the getting started
//       guide does not match the Braccio.begin() code. These angles are the
//       default position as given in the Braccio.h library with some
//       modification. Note the init does not set everything the same as this 
//       for some reason. Notes in the braccio library.
#define M1_SAFE_ANGLE 0
#define M2_SAFE_ANGLE 40
#define M3_SAFE_ANGLE 180
#define M4_SAFE_ANGLE 170
#define M5_SAFE_ANGLE 90
#define M6_SAFE_ANGLE 73

typedef struct parsed_io_msg {
    uint8_t msg_size;
    uint8_t msg_type;
    uint8_t cmd;
    uint8_t param[PARAM_BUFF];
    uint8_t param_len;
} parsed_msg_s;

typedef struct io_msg {
    uint8_t msg[IO_MSG_BUFF];
    uint8_t len;
} io_msg_s;

#define NUM_ANGLES 6 // m[1-6]
typedef struct braccio_angles {
    uint8_t m1;
    uint8_t m2;
    uint8_t m3;
    uint8_t m4;
    uint8_t m5;
    uint8_t m6;
} braccio_angles_s;

class braccio_arm {
    public:
        braccio_arm(Stream &serial); 
        ~braccio_arm();

        void init_arm(int soft_start_level=SOFT_START_DEFAULT); 
        void set_default_pos();

        bool serial_avail();
        int serial_read(uint8_t *buff, size_t len);
    
        int set_parsed_msg(parsed_msg_s *fill, uint8_t msg_type, uint8_t cmd,
                           uint8_t param_len, uint8_t *param);
        
        int parse_msg(uint8_t *msg, parsed_msg_s *in_msg);
        int exec_command(parsed_msg_s *in_msg);
        
        void send_ack();
        void send_finish();
        int send_print(const char *format, ...);
        int send_verbose(const char *format, ...);
        int send_error(const char *format, ...);
        int send_all_angles(); 

        int create_send_msg(parsed_msg_s *msg);
        int create_io_msg(parsed_msg_s *msg, io_msg_s *io_msg);
        int send_message(io_msg_s *msg);

    private:
        uint8_t check_angle(uint8_t angle, uint8_t min, uint8_t max);
        void check_all_angles(uint8_t a1, uint8_t a2, uint8_t a3, uint8_t a4,
                              uint8_t a5, uint8_t a6);
        int vsnprintf_check(char *buff, int size, const char *format, 
                            va_list args); 
         
        Stream &serial;
        braccio_angles_s angles;

        /*
         * NOTE: There is a _Braccio class object declared as extern globably in
         *       Braccio.h named Braccio. We must declare our servos globally
         *       in the .cpp file for externs in Braccio.cpp. 
         *       I commented out the global braccio class extern in the llibrary
         *       header and declared my own variable here.
         *       If you downloaded Braccio.h via arduino IDE it is usually
         *       in ~/Arduino/libraries/
         *
         *       You do not need to comment it out for this to work but it may
         *       take up space.
         */

        _Braccio braccio;
};
#endif

braccio_arm.cpp

#include "braccio_arm.h"

#define SUCCESS 0
#define FAILURE -1

// these must be declared for extern variables in the Braccio library
// in Braccio.cpp 
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;

braccio_arm::braccio_arm(Stream &serial): serial(serial)
{
    // set default angles into saftey position
    angles.m1 = M1_SAFE_ANGLE;
    angles.m2 = M2_SAFE_ANGLE;
    angles.m3 = M3_SAFE_ANGLE;
    angles.m4 = M4_SAFE_ANGLE;
    angles.m5 = M5_SAFE_ANGLE;
    angles.m6 = M6_SAFE_ANGLE;
}

braccio_arm::~braccio_arm()
{
}

// initialize robot arm to default position and sets variables in braccio
void braccio_arm::init_arm(int soft_start_level)
{
    send_verbose("Starting Braccio\n");
    braccio.begin(soft_start_level);
    set_default_pos();
}

void braccio_arm::set_default_pos()
{
    send_verbose("setting default position\n");

    angles.m1 = M1_SAFE_ANGLE;
    angles.m2 = M2_SAFE_ANGLE;
    angles.m3 = M3_SAFE_ANGLE;
    angles.m4 = M4_SAFE_ANGLE;
    angles.m5 = M5_SAFE_ANGLE;
    angles.m6 = M6_SAFE_ANGLE;

    braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                          angles.m4, angles.m5, angles.m6);

    send_verbose("default position set\n");
}

bool braccio_arm::serial_avail()
{
    if (serial.available())
        return true;
    return false;
}

int braccio_arm::serial_read(uint8_t *buff, size_t len)
{
    int num_recv;
    num_recv = serial.readBytes(buff, len); // room for '\0'
    return num_recv;
}

int braccio_arm::send_all_angles()
{
    parsed_msg_s out_msg;
    uint8_t param[NUM_ANGLES] = {angles.m1, angles.m2, angles.m3, angles.m4,
                                 angles.m5, angles.m6};

    set_parsed_msg(&out_msg, CMD_MSG, SEND_ANGLES, NUM_ANGLES, param);
    if (errno) {
        send_error("Failed to set parameters in parsed message.\n"); 
        errno = SUCCESS;
        return FAILURE;   
    }
/* ERROR HERE: Uncommented create_send_msg causes   error.
    create_send_msg(&out_msg);
    if (errno) {
        send_error("Failed to create and send message");
        errno = SUCCESS;
        return FAILURE;
    }
*/
    return SUCCESS;
}

void braccio_arm::send_ack()
{
    uint8_t ack[2] = {1, ACK};
    serial.write(ack, 2);
}

void braccio_arm::send_finish()
{
    uint8_t finish[2] = {1, FINISH};
    serial.write(finish, 2);
}

int braccio_arm::send_print(const char *format, ...)
{
    va_list args;
    uint8_t set_msg[PARAM_BUFF];
    parsed_msg_s out_msg;

    va_start(args, format);

    vsnprintf_check((char*)set_msg, PARAM_BUFF, format, args);
    set_parsed_msg(&out_msg, PRINT_MSG, PRINT_GENERAL, 
                   PARAM_STRLEN, set_msg);
    if (errno) {
        send_error("Failed to set parameters in parsed message.\n");
        errno = SUCCESS;
        return FAILURE;   
    }

    create_send_msg(&out_msg);
    if (errno) {
        send_error("Failed to create and send message");
        errno = SUCCESS;
        return FAILURE;
    }

    va_end(args);

    return SUCCESS;
}


int braccio_arm::send_error(const char *format, ...)
{
    va_list args;
    uint8_t set_msg[PARAM_BUFF];
    parsed_msg_s out_msg;

    va_start(args, format);

    vsnprintf_check((char*)set_msg, PARAM_BUFF, format, args);
    set_parsed_msg(&out_msg, PRINT_MSG, PRINT_ERROR, 
                   PARAM_STRLEN, set_msg);
    if (errno) {
        send_error("Failed to set parameters in parsed message.\n"); 
        errno = SUCCESS;
        return FAILURE;   
    }

    create_send_msg(&out_msg);
    if (errno) {
        send_error("Failed to create and send message\n");
        errno = SUCCESS;
        return FAILURE;
    }

    va_end(args);

    return SUCCESS;
}

int braccio_arm::send_verbose(const char *format, ...)
{
    va_list args;
    uint8_t set_msg[PARAM_BUFF];
    parsed_msg_s out_msg;

    va_start(args, format);

    vsnprintf_check((char*)set_msg, PARAM_BUFF, format, args);

    set_parsed_msg(&out_msg, PRINT_MSG, PRINT_VERBOSE, 
                   PARAM_STRLEN, set_msg);
    if (errno) {
        send_error("Failed to set parameters in parsed message.\n");
        errno = SUCCESS;
        return FAILURE;   
    }

    create_send_msg(&out_msg);
    if (errno) {
        send_error("Failed to create and send message\n");
        errno = SUCCESS;
        return FAILURE;
    }

    va_end(args);

    return SUCCESS;
}

int braccio_arm::set_parsed_msg(parsed_msg_s *fill, uint8_t msg_type, 
                                 uint8_t cmd, uint8_t param_len, 
                                 uint8_t *param)
{
    int i;
    int k;
    
    fill->msg_type = msg_type;
    fill->cmd = cmd;
    
    // If i dont do fill->param_len here compiler complains it may not be init
    fill->param_len = param_len;
    if (param_len == PARAM_STRLEN) { // treat param as string of variable length
        param_len = strnlen((char*)param, PARAM_BUFF); // excludes '\0'
        if (param_len == PARAM_BUFF && param[param_len-1] != '\n')
            return errno = EINVAL;
    } else if (param_len > PARAM_BUFF) {
        return errno = EINVAL;
    }

    fill->param_len = param_len;
    for (i=0, k=0; i < param_len; ++i, ++k)
        fill->param[i] = param[k];
        
    // exclude msg_size in size so reciever can parse 1 byte then parse the rest
    fill->msg_size = param_len + MSG_SIZE_NO_PARAM - 1; 

    return SUCCESS;
}

int braccio_arm::parse_msg(uint8_t *msg, parsed_msg_s *in_msg)
{
    uint8_t i = 0;
    uint8_t k = 0;

    // parse msg type
    in_msg->msg_type = msg[i++]; 

    // parse cmd type
    in_msg->cmd = msg[i++];
    
    // parse parameter length
    in_msg->param_len = msg[i++];

    if (in_msg->param_len > PARAM_BUFF)
        return errno = EINVAL;
    
    // parse parameters
    for (k=0; k < in_msg->param_len; ++k)
        in_msg->param[k] = msg[i++];
    
    // this is not sent by the python code but is set for initialization
    in_msg->msg_size = in_msg->param_len + MSG_SIZE_NO_PARAM - 1;

    return SUCCESS;
}

int braccio_arm::exec_command(parsed_msg_s *in_msg)
{
    if (in_msg->msg_type != CMD_MSG)
        return errno = EINVAL;

    switch (in_msg->cmd) {
    case M1_ANGLE:
        angles.m1 = check_angle(in_msg->param[0], M1_MIN_ANGLE, M1_MAX_ANGLE);
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M1 angle to %u\n", angles.m1);
    break;
    case M2_ANGLE:
        angles.m2 = check_angle(in_msg->param[0], M2_MIN_ANGLE, M2_MAX_ANGLE); 
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M2 angle to %u\n", angles.m2);
    break;
    case M3_ANGLE:
        angles.m3 = check_angle(in_msg->param[0], M3_MIN_ANGLE, M3_MAX_ANGLE); 
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M3 angle to %u\n", angles.m3);
    break;
    case M4_ANGLE:
        angles.m4 = check_angle(in_msg->param[0], M4_MIN_ANGLE, M4_MAX_ANGLE); 
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M4 angle to %u\n", angles.m4);
    break;
    case M5_ANGLE:
        angles.m5 = check_angle(in_msg->param[0], M5_MIN_ANGLE, M5_MAX_ANGLE); 
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M5 angle to %u\n", angles.m5);
    break;
    case M6_ANGLE:
        angles.m6 = check_angle(in_msg->param[0], M6_MIN_ANGLE, M6_MAX_ANGLE); 
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed M6 angle to %u\n", angles.m6);
    break;
    case MX_ANGLE:
        check_all_angles(in_msg->param[0], in_msg->param[1], in_msg->param[2],
                         in_msg->param[3], in_msg->param[4], in_msg->param[5]);
        braccio.ServoMovement(DFLT_STEP_DELAY, angles.m1, angles.m2, angles.m3,
                              angles.m4, angles.m5, angles.m6);
        send_verbose("Changed all angles, "
                     "M1: %d, M2: %d, M3: %d, M4: %d, M5: %d, M6: %d\n", 
                     angles.m1, angles.m2, angles.m3, angles.m4, angles.m5,
                     angles.m6);
    break;
    case REQUEST_MX_ANGLE:
        send_all_angles(); 
        send_verbose("Sent all angles, "
                     "M1: %d, M2: %d, M3: %d, M4: %d, M5: %d, M6: %d\n", 
                     angles.m1, angles.m2, angles.m3, angles.m4, angles.m5,
                     angles.m6);
    break; 
    default:
        send_verbose("Invalid command recieved.\n");
        return errno = EINVAL;
    }

    return SUCCESS;
}

int braccio_arm::create_send_msg(parsed_msg_s *msg)
{
    io_msg_s to_send;

    if (create_io_msg(msg, &to_send))
        return FAILURE;

    send_message(&to_send);
    if (errno)
        return FAILURE;

    return SUCCESS;
}

int braccio_arm::create_io_msg(parsed_msg_s *msg, io_msg_s *io_msg)
{
    uint8_t i = 0;
    uint8_t k = 0;

    io_msg->msg[i++] = msg->msg_size;
    io_msg->msg[i++] = msg->msg_type;
    io_msg->msg[i++] = msg->cmd;
    
    if (msg->param_len > PARAM_BUFF)
        return errno = EINVAL;

    io_msg->msg[i++] = msg->param_len;

    for (k=0; k < msg->param_len; ++k)
        io_msg->msg[i++] = msg->param[k];
    
    io_msg->len = MSG_SIZE_NO_PARAM + k;

    return SUCCESS;
}

int braccio_arm::send_message(io_msg_s *msg)
{
    int ret;

    ret = serial.write(msg->msg, msg->len);
    if (ret != msg->len) {
        errno = EIO;
        return FAILURE;
    }

    return ret;
}

                    /* private functions */

uint8_t braccio_arm::check_angle(uint8_t angle, uint8_t min, uint8_t max)
{
    if (angle < min)
        return min;

    if (angle > max)
        return max;

    return angle;
}

void braccio_arm::check_all_angles(uint8_t a1, uint8_t a2, uint8_t a3, 
                                   uint8_t a4, uint8_t a5, uint8_t a6)
{
    angles.m1 = check_angle(a1, M1_MIN_ANGLE, M1_MAX_ANGLE);
    angles.m2 = check_angle(a2, M2_MIN_ANGLE, M2_MAX_ANGLE); 
    angles.m3 = check_angle(a3, M3_MIN_ANGLE, M3_MAX_ANGLE); 
    angles.m4 = check_angle(a4, M4_MIN_ANGLE, M4_MAX_ANGLE); 
    angles.m5 = check_angle(a5, M5_MIN_ANGLE, M5_MAX_ANGLE); 
    angles.m6 = check_angle(a6, M6_MIN_ANGLE, M6_MAX_ANGLE);
}

int braccio_arm::vsnprintf_check(char *buff, int size, const char *format,
                                 va_list args)
{
    int num_written = 0;

    num_written = vsnprintf(buff, size, format, args);
    if (num_written > size-1) { // format was too large for buffer
        send_error("Future message truncated, message was to long.");
        return FAILURE;
    }

    return SUCCESS;
}

command_exec_test.ino (main)

/*
 * Program to test sending and recieving data from serial going through
 * a command interface with braccio robot arm
 * written by: James Ross
 */

#include "braccio_arm.h"

#define BAUD_RATE 115200
#define S_IN_BUFF 20 // serial input buffer size

uint8_t serial_in[S_IN_BUFF] = {'\0'};

braccio_arm braccio = braccio_arm(Serial);

void setup()
{ 
    // Serial.begin must be here since stream object doesnt contain begin()
    Serial.begin(BAUD_RATE);
    while (!Serial) {} // wait for serial interace to connect
    
    // initialize the robot arm, goes to default position
    braccio.init_arm();
    
    braccio.send_verbose("Setup Complete\n");
}

void loop()
{
    parsed_msg_s parsed_msg;
    if (braccio.serial_avail()){
        braccio.serial_read(serial_in, S_IN_BUFF);
        braccio.send_ack();
        braccio.parse_msg(serial_in, &parsed_msg);
        braccio.exec_command(&parsed_msg);
        braccio.send_finish();
    }
}

i know its alot of posted code, hopefully its not too hard to find the functions. I have a theory its about size but i am unsure since the dynamic memory goes from 38% to 40% but it does not even get to allocated alot of local variables.

Any ideas would be great. I am getting an Arduino Due to test it on as well.

groundFungus · November 11, 2021, 7:13pm

The symptom (repeatedly resetting, etc.) that you describe can be caused by writing or reading outside the bounds of an array. Are there any array operations in the suspect function?

sedihglow · November 11, 2021, 7:16pm

It does not reach the function when it runs before resetings or sending garbage. So i dont expect it to be running over bounds of arrays.

It basically reaches braccio.init_arm() and ends there in the program.

send_all_angles() only gets called when my laptop sends a message over serial to request the angles. So it is not even reaching this function.

sedihglow · November 11, 2021, 7:21pm

If i uncommented create_send_msg() in send_all_angles() the program gets to braccio.init_arm() and stops. The robot arm does not move tho.

Without send_all_angles() or without create_send_msg() inside of send_all_angles() the program runs just fine. All functions work. Thats the best way i can describe the problem for now.

send_all_angles() is not being called and the problem still occures.

The problem is the arudino is reseting and sending garbage over the serial and not running through the program passed braccio.init_arm() which works just fine if send_all_angles() isnt in the program.

sedihglow · November 11, 2021, 7:26pm

I dont think the issue is with braccio.init_arm since it works just fine and has been working fine for a long time now. I havent changed anything in that function.

The only change is creating the function send_all_angles() which i pinned down to the function call create_send_msg().

create_send_msg() works in other functions.

if i do not include send_all_angles() in my code the program runs just fine.

This is why its so weird. Its not calling send_all_angles() and the issue occures. Ive never seen anything quite like it.

Just double checked the behavior. Nothing happens with the robot arm and its sending garbage over serial. It does not print the send_verbose message in braccio.init_arm()

Idahowalker · November 11, 2021, 7:32pm

Running out of RAM or going over the boundary of an array.

sedihglow · November 11, 2021, 9:02pm

I must be running out of RAM. I reduced some buffer sizes and it works.

Thanks for helping me out. I need this function done and nothing else too unfortunately lol.

I will test it out on my Arduino Due to see if it has enough.

Thanks for working it out with me.

system · May 10, 2022, 9:02pm

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