Rosserial unable to sync with device due to else statement? - Arduino uno

asked 2016-10-06 13:48:06 -0600

215 gravatar image

updated 2016-10-06 14:27:49 -0600

I am currently unable to connect with rosserial, I began debugging the issue, and seem to have found the cause of the problem, but don't understand why this is the problem in the first place.

Here is my code:

speed_profile.cpp

#include "speed_profile.h"


speed_profile profile;
ros::NodeHandle nh;

std_msgs::Int16 status_step_count;
ros::Publisher chatter("tipper_status", &status_step_count);

bool global_state = false;;
void call_back( const std_msgs::Empty & msg)
{
  global_state = true;
}

ros::Subscriber<std_msgs::Empty> sub("tipper_control", &call_back );

void output_pin_setup()
{
  pinMode(en_pin,OUTPUT);
  pinMode(dir_pin,OUTPUT);
  pinMode(step_pin,OUTPUT); 
  nh.initNode();
  nh.advertise(chatter);
  nh.subscribe(sub);
  //delay(1000);
  //nh.getHardware()->setBaud(57600);

}


void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
{
  if(global_state == false)
  {
    //do nothing
  }
  //else -- HERE IS THE PROBLEM
  //{  
    digitalWrite(en_pin,HIGH);
    delay(1);

    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 (motor_steps < 0)
    {
      profile.dir = CCW;
      motor_steps = -motor_steps;
      digitalWrite(dir_pin,LOW);
    }
    else
    {
      profile.dir = CW;
      digitalWrite(dir_pin,HIGH);
    }

    delay(1);

  // 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 = steps_to_speed - motor_steps;
        profile.decel_length = -(motor_steps-acceleration_step_limit); //---
      }
      else 
      {
        //profile.decel_length = -((long)steps_to_speed * acceleration_step_limit) / motor_decel;acceleration_step_limit
        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.
      //int steps_in_run = motor_steps - steps_to_speed-profile.decel_length;

      profile.decel_start = motor_steps + profile.decel_length;
    //profile.decel_start = motor_steps - steps_to_speed-steps_in_run;

    // 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;
      OCR1A = 10;
    // Set ...
(more)
edit retag flag offensive close merge delete