0
$\begingroup$

Rosanswers logo

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 by calc the first (c0) step delay .
    // first_step_delay = 1/tt * sqrt(2*alpha/accel)
    // first_step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000
    profile.first_step_delay = (T1_FREQ_148 * sqrt(A_SQ / motor_accel)) / 100;

    // Find out after how many steps does the speed hit the max speed limit.
    // steps_to_speed = speed^2 / (2*alpha*accel)
    steps_to_speed = (long)motor_speed * motor_speed / (long)(((long)A_x20000 * motor_accel) / 100);
    // If we hit max speed limit before 0,5 step it will round to 0.
    // But in practice we need to move atleast 1 step to get any speed at all.
    if (steps_to_speed == 0)
    {
      steps_to_speed = 1;
    }

    // Find out after how many steps we must start deceleration.
    // n1 = (n1+n2)decel / (accel + decel)
    acceleration_step_limit = ((long)motor_steps * motor_decel) / (motor_accel + motor_decel);
    // We must accelrate at least 1 step before we can start deceleration.
    if (acceleration_step_limit == 0)
    {
      acceleration_step_limit = 1;
    }

    // Use the limit we hit first to calc decel.
    if (acceleration_step_limit <= steps_to_speed)
    {
      profile.decel_length = -(motor_steps - acceleration_step_limit); //---
    }
    else
    {
      profile.decel_length = -((long)steps_to_speed * motor_accel) / motor_decel; //--
    }
    // We must decelrate at least 1 step to stop.
    if (profile.decel_length == 0)
    {
      profile.decel_length = -1;
    }

    // Find step to start decleration.
    profile.decel_start = motor_steps + profile.decel_length;

    // If the maximum speed is so low that we dont need to go via accelration state.
    if (profile.first_step_delay <= profile.min_time_delay)
    {
      profile.first_step_delay = profile.min_time_delay;
      profile.run_state = RUN;
    }
    else
    {
      profile.run_state = ACCEL;
    }

    // Reset counter.
    profile.accel_count = 0;
    //profile.step_counter = steps_to_speed;
  }
}

void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
{
  while (global_state == false)
  {
    //do nothing
    status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
    status_tipper_availability.data = 0;
    
    chatter_status.publish( &status_step_count);
    chatter_availabilty.publish(&status_tipper_availability);
    profile.debug = 0;
    nh.spinOnce();
  }

  digitalWrite(en_pin, HIGH);
  delay(1);

  signed int move_steps;
  //if (motor_steps < 0)
  if(received_data > profile.current_step_position) // Compares whether received percentage (converted to motor_steps) is greater or smaller than current_step_position.
  {
    profile.dir = CCW;
    //motor_steps = -motor_steps;
    move_steps =  profile.current_step_position - received_data;
    digitalWrite(dir_pin, HIGH);                          
  }
  else
  {
    profile.dir = CW;
    move_steps =  profile.current_step_position - received_data;
    digitalWrite(dir_pin, LOW);
  }

  delay(1);
  // received_data = percentage converted to a step number received from tipper_control topic
  
  computation_of_speed_profile(move_steps, motor_accel, motor_decel, motor_speed); // function should be called with the correct motor_steps value

  OCR1A = 10;
  // Set Timer/Counter to divide clock by 8
  TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10));


  while (global_state == true)
  { 
    status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
    status_tipper_availability.data = profile.first_step_delay;

    chatter_availabilty.publish(&status_tipper_availability);
    chatter_status.publish( &status_step_count);
    nh.spinOnce();
    //delay(100);
  }
}

ISR(TIMER1_COMPA_vect)
{
  // Holds next delay period.
  unsigned int new_first_step_delay;

  // Remember the last step delay used when accelrating.
  static int last_accel_delay;

  // Counting steps when moving.
  static unsigned int step_count = 0;

  // Keep track of remainder from new_step-delay calculation to incrase accurancy
  static unsigned int rest = 0;
  OCR1A = profile.first_step_delay;
  switch (profile.run_state)
  {

    case STOP:
      step_count = 0;
      global_state = false;
      rest = 0;
      TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); // Stop the timer,  No clock source
      break;

    case ACCEL:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      PORTB ^= _BV(PB3);  // Toggles the step_pin
      step_count++;
      
      if (profile.dir == CCW)
      {
        profile.current_step_position++;
      }
      else
      {
        profile.current_step_position--;
      }  
      
      profile.accel_count++;
      new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1));
      rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1);

      // Chech if we should start decelration.
      if (step_count >= profile.decel_start)
      {
        profile.accel_count = profile.decel_length;
        profile.run_state = DECEL;
      }

      // Chech if we hitted max speed.
      else if (new_first_step_delay <= profile.min_time_delay)
      {
        last_accel_delay = new_first_step_delay;
        new_first_step_delay = profile.min_time_delay;
        rest = 0;
        profile.run_state = RUN;
      }
      break;
    case RUN:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      PORTB ^= _BV(PB3); // Toggles the step_pin
      step_count++;
      
      if (profile.dir == CCW)
      {
        profile.current_step_position++;
      }
      else
      {
        profile.current_step_position--;
      }  
      new_first_step_delay = profile.min_time_delay;
      // Chech if we should start decelration.
      if (step_count >= profile.decel_start)
      {
        profile.accel_count = profile.decel_length;
        // Start decelration with same delay as accel ended with.
        new_first_step_delay = last_accel_delay;
        profile.run_state = DECEL;
      }
      break;
    case DECEL:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      PORTB ^= _BV(PB3); // Toggles the step_pin
      step_count++;
      if (profile.dir == CCW)
      {
         profile.current_step_position++;
      }
      else
      {
        profile.current_step_position--;
      }  
      profile.accel_count++;
      new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1));
      rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1);
      // Check if we at last step
          
      if (profile.accel_count >= 0)
      {
        digitalWrite(en_pin, !digitalRead(en_pin));
        //PORTB &= ~_BV(PB3);

        profile.run_state = STOP;
      }
      break;

  }
   
  profile.first_step_delay = new_first_step_delay;
  
}

Originally posted by 215 on ROS Answers with karma: 156 on 2016-10-10

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Mark Rose with karma: 1563 on 2016-10-10

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by 215 on 2016-10-10:
I am currently running out of memory, meaning i have 553 bytes left.

Comment by 215 on 2016-10-10:
using the internal class gave me 949 bytes of local variables left, so it should be ok?

Comment by 215 on 2016-10-10:
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.

Comment by 215 on 2016-10-11:
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.

Comment by Mark Rose on 2016-10-11:
Sorry for the confusion. It really depends on what messages you're sending/receiving. Glad you got it working.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.