I am currently working with interfacing my uno with ROS, and seem to have problems communicating if I add a certain if/else statement.
I am not whether it is because something is being stalled? but rosserial (the rospart that communicates with the uno) can’t communicate with it this if/else statement is in this function.
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 //// THIS ELSE - if this is uncommented rosserial will not be able to communicate with it.
//{
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 Timer/Counter to divide clock by 8
TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10));
profile.moved_steps = 0;
while(1)
{
cli();
status_step_count.data = profile.moved_steps;
sei();
chatter.publish( &status_step_count);
nh.spinOnce();
delay(1);
}
//}
}
}
The function is called from main as such.
#include "speed_profile.h"
void setup() {
// put your setup code here, to run once:
cli();
output_pin_setup();
timer1_setup();
sei();
}
void loop()
{
int motor_steps = -3000;//-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
compute_speed_profile(motor_steps, motor_acceleration, motor_deceleration, motor_speed);
while(1)
{
}
}
I am guessing something is stalling? but can’t quite figure out what should be stalled in the program?