Rosserial unable to sync with device due to else statement? - Arduino uno
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 ...