Ask Your Question
0

Why is rosserial skipping part of the code?

asked 2016-10-10 10:29:23 -0500

215 gravatar image

updated 2016-10-10 10:30:35 -0500

I am currently using rosserial to interface with my arduino, but it seems like that is it skipping some parts of the code?

This is my main.ino

#include "speed_profile.h"
void setup() {
  // put your setup code here, to run once:
  output_pin_setup();
  cli();
  timer1_setup();
  sei();
}

void loop() 
{
      int motor_steps = 23400;//-9600; //23400 -  100%
      // Accelration to use.
      int motor_acceleration = 500;//500;
      // Deceleration to use.
      int motor_deceleration = 500;//500;
      // Speed to use.
      int motor_speed = 1000; // 1000
      init_tipper_position();
      compute_speed_profile(motor_steps, motor_acceleration, motor_deceleration, motor_speed); 
}

The function init_tipper_position() initializes the position of my motor, but for some reason is this function ignored. It never gets executed which i can feel on the motor as i am able to move it due to missing en_pin being high.

The code jumps directly to compute_speed_profile() when i start rosserial, and wait for me to publish something on to the topic it subscribes, and moves when I provide it the thing it needs.

how come is it skipping the prior function?

This is the init_tipper_position() function

void init_tipper_position()
{
  digitalWrite(en_pin, HIGH);
  delay(1);

  digitalWrite(dir_pin, LOW);
  delay(1);

  int decrement = 0;
  while((PINB & (1 << 2)))  
  {
    decrement++;
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(600);
    digitalWrite(step_pin, LOW);
    delayMicroseconds(600);
  }

  digitalWrite(en_pin, LOW);

}

The other function declarations can be seen in the attached .

.cpp file

#include "speed_profile.h"


volatile speed_profile profile;

ros::NodeHandle nh;

std_msgs::Int16 status_step_count;
std_msgs::Int16 status_tipper_availability;
ros::Publisher chatter_status("tipper_status", &status_step_count);
ros::Publisher chatter_availabilty("tipper_availability", &status_tipper_availability);

volatile bool global_state = false;
bool moved_to_position = true;
int received_data = 0;


void call_back_control( const std_msgs::Empty & msg)
{
  global_state = true;
  // For HMI
  received_data  = (10 *23400.0)/100.0; // Converts input to motor_steps.
}

ros::Subscriber<std_msgs::Empty> sub_control("tipper_control", &call_back_control );

void output_pin_setup()
{
  pinMode(en_pin, OUTPUT);
  pinMode(dir_pin, OUTPUT);
  pinMode(step_pin, OUTPUT);
  pinMode(slot_sensor_pin,INPUT_PULLUP);
  nh.initNode();
  nh.advertise(chatter_status);
  nh.advertise(chatter_availabilty);
  nh.subscribe(sub_control);
  //nh.subscribe(sub_command);
  profile.current_step_position = 0;
  delay(10);
  nh.getHardware()->setBaud(57600);
}

void init_tipper_position()
{
  digitalWrite(en_pin, HIGH);
  delay(1);

  digitalWrite(dir_pin, LOW);
  delay(1);

  int decrement = 0;
  while((PINB & (1 << 2)))  
  {
    decrement++;
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(600);
    digitalWrite(step_pin, LOW);
    delayMicroseconds(600);
  }

  digitalWrite(en_pin, LOW);

}
void timer1_setup()
{
  // Tells what part of speed ramp we are in.
  profile.run_state = STOP;

  // Timer/Counter 1 in mode 4 CTC (Not running).
  TCCR1B = (1 << WGM12);

  // Timer/Counter 1 Output Compare A Match Interrupt enable.
  TIMSK1 = (1 << OCIE1A);
}

void computation_of_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
{
  unsigned int steps_to_speed;  // Steps required to achieve the the speed desired
  unsigned int acceleration_step_limit; // If desired speed is not achieved, will this variable contain step_limit to when to stop accelerating.
  // If moving only 1 step.
  if (motor_steps == 1)
  {
    // Move one step
    profile.accel_count = -1;
    // in DECEL state.
    profile.run_state = DECEL;
    // Just a short delay so main() can act on 'running'.
    profile.first_step_delay = 1000;
    OCR1A = 10;
    // Run Timer/Counter 1 with prescaler = 8.
    TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10));
  }
  else if (motor_steps != 0)
  {
    // Set max speed limit, by calc min_delay to use in timer.
    // min_delay = (alpha / tt)/ w
    profile.min_time_delay = A_T_x100 / motor_speed;

    // Set accelration ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-10-10 11:41:29 -0500

Mark Rose gravatar image

Without knowing more about your setup it's hard to know what's wrong. When you compile your sketch, how much space does the compiler say is left for local variables? ("leaving nnn bytes for local variables")

I've had very mixed results with rosserial_arduino on 2kb RAM Arduino. My sketches failed to work reliably unless at least 700 bytes or so of "local variable" space was left. Remember that "local variables" means stack+heap.

There are some things you can do to reduce memory usage (a little). Instead of declaring your node handle like this:

ros::NodeHande nh;

declare it instead using an internal class and specify the number of publishers and subscribers and the input and output buffer sizes explicitly:

ros::NodeHandle_<ArduinoHardware, 5, 14, 125, 125> nh;

The parameters, in order, are the number of subscribers you will declare, the number of publishers, the size of the input buffer, in bytes, and the size of the output buffer, in bytes. The input buffer must be large enough to hold any incoming messages for one call to nh::spinOnce(), and the output buffer must be large enough to hold all messages published in between calls to nh::spinOnce(). On my robot I was trying to publish 14 sensor values. The "125" values for the buffer sizes were are the smallest I was able to get to work. Even at that I had only about 600 bytes free, and the system proved unreliable.

I've abandoned rosserial_arduino because of the difficulty of making it work on a 2kbyte Arduino. A larger device such as a Mega or Teensy would likely be fine. Instead, I'm using the ros_arduino_bridge package for Linux/Arduino communication.

edit flag offensive delete link more

Comments

I am currently running out of memory, meaning i have 553 bytes left.

215 gravatar image215 ( 2016-10-10 12:00:31 -0500 )edit

using the internal class gave me 949 bytes of local variables left, so it should be ok?

215 gravatar image215 ( 2016-10-10 12:13:09 -0500 )edit

I found the issue somewhere else in the code.. In the while, but i will keep this answer, because i didn't know about the nodehandle class and other people might benefit from it.

215 gravatar image215 ( 2016-10-10 15:44:13 -0500 )edit

After further inspection, and after a lot of debugging it seems like what you mention the buffer size has been low.. I doubled it to 256 which seem to have worked. I guess I am currently at the limit of what I can put in with only 683 byte left.

215 gravatar image215 ( 2016-10-11 13:14:56 -0500 )edit

Sorry for the confusion. It really depends on what messages you're sending/receiving. Glad you got it working.

Mark Rose gravatar imageMark Rose ( 2016-10-11 13:32:21 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2016-10-10 10:29:23 -0500

Seen: 139 times

Last updated: Oct 10 '16