Variable not incrementing?
I am at the moment trying to communicate between my arduino uno and ROS (rosserial) such that an outside node can connect, and ask for a service.
I currently able to make them communicate (which entail that I have have made a publisher and subscriber which other nodes can sent to)
The information that should be published is how far my motor has moved, which is incremented in a ISR routine. Problem is that when an outside node is asking for a service, does the publishing node print the current position of the motor, which currently always zero. The actual service is also never performed, meaning that the program never enters the stage where it should perform something.
I suspect the error to be in the .cpp file,
void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
It seems like even though the globalstate flags is becoming true, is it not going into to the else part of the if/else statement, and thereby not making the motor, and the movedstep count increment. How do fix this else that does not respond to the state of the global_state bool?
This is the .cpp which contains the function declarations and the ISR routine:
.cpp - function
#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);
profile.moved_steps = 0;
//delay(1000);
nh.getHardware()->setBaud(57600);
}
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 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 //---- ------- ERROR MIGHT BE DUE TO THIS
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));
while(1)
{
cli();
status_step_count.data = profile.moved_steps;
sei();
chatter.publish( &status_step_count);
nh.spinOnce();
delay(1);
}
}
}
Asked by 215 on 2016-10-06 15:34:19 UTC
Comments