Why is my ISR not completing, and stalling the program?

Code C - [expand]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#include "speed_profile.h"
 
void setup() {
  // put your setup code here, to run once:
  output_pin_setup();
  cli();
  timer1_setup();
  sei();
}
 
void loop() 
{
    init_tipper_position();
      compute_speed_profile(23400, 500, 500, 1000); 
}

Code C - [expand]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
#include "speed_profile.h"
 
 
volatile speed_profile profile;
 
ros::NodeHandle_<ArduinoHardware, 1, 2, 125, 125> 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;
int received_data = 0;
 
 
void call_back_control( const std_msgs::Empty & msg)
{
  global_state = true;
  // For HMI
  received_data  = (100 *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);
  profile.debug = 0;
  profile.current_step_position = 0;
  delay(10);
  nh.getHardware()->setBaud(57600);
}
 
void init_tipper_position()
{
 
 
  
  while(!(PINB & (1 << 2)))  
  {
    // Waits until sensor gets activated
  }
 
 
}
void timer1_setup()
{
  // Tells what part of speed ramp we are in.
  profile.run_state = STOP;
  
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;  // reset the timer
  
  // 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;
  }
}
 
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 = profile.current_step_position;
    status_tipper_availability.data = 1;
    
    chatter_status.publish( &status_step_count);
    chatter_availabilty.publish(&status_tipper_availability);
    nh.spinOnce();
  }
 
  digitalWrite(en_pin, HIGH);
  delay(1);
 
  signed int move_steps;
 
  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;
    move_steps =  received_data - profile.current_step_position;
    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));
 
  //unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate.  = -28499
  //unsigned int decel_start; //  Determines at which step the deceleration should begin. =  20836
  //signed int decel_length; // Set the deceleration length =  0
  //signed int min_time_delay; //minium time required time_delay for achieving the wanted speed.  = 1
  //signed int accel_count; // counter used when computing step_delay for acceleration/decelleration. =  0
 
  while (global_state == true)
  { 
    status_step_count.data = profile.current_step_position; 
    status_tipper_availability.data = 0;
 
    chatter_availabilty.publish(&status_tipper_availability);
    chatter_status.publish( &status_step_count);
    nh.spinOnce();
  }
}
 
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:
      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:
      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:
      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)
      {
        PORTB &= ~_BV(PB3);
        profile.run_state = STOP;
        status_tipper_availability.data = 2;
      chatter_availabilty.publish(&status_tipper_availability);
      }
      break;
 
  }
   
  profile.first_step_delay = new_first_step_delay;
  
}

Code C - [expand]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
/*
 * Contains the class concerning with calculating the proper speed profile 
 * for accelerating and decelerating the stepper motor.
 * 
 */
 
#ifndef speed_profile_h
#define speed_profile_h
 
 
#include <Arduino.h> 
#include <ros.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Empty.h>
 
// Timer/Counter 1 running on 3,686MHz / 8 = 460,75kHz (2,17uS). (T1-FREQ 460750)
//#define T1_FREQ 460750
#define T1_FREQ 1382400
 
//! Number of (full)steps per round on stepper motor in use.
#define FSPR 1600
 
// Maths constants. To simplify maths when calculating in compute_speed_profile().
#define ALPHA (2*3.14159/FSPR)                    // 2*pi/spr
#define A_T_x100 ((long)(ALPHA*T1_FREQ*100))     // (ALPHA / T1_FREQ)*100
#define T1_FREQ_148 ((int)((T1_FREQ*0.676)/100)) // divided by 100 and scaled by 0.676
#define A_SQ (long)(ALPHA*2*10000000000)         // ALPHA*2*10000000000
#define A_x20000 (int)(ALPHA*20000)              // ALPHA*20000
 
// Speed ramp states
#define STOP  0
#define ACCEL 1
#define DECEL 2
#define RUN   3
 
// Pin numbering
#define en_pin 13
#define dir_pin 12
#define step_pin 11
#define slot_sensor_pin 10
 
// Motor direction 
#define CW  0
#define CCW 1
 
typedef struct 
{
  unsigned char run_state : 3; // Determining the state of the speed profile
  unsigned char dir: 1; // Determining the direction the motor has to move - Start being CCW 
  unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate. 
  unsigned int decel_start; //  Determines at which step the deceleration should begin. 
  signed int decel_length; // Set the deceleration length
  signed int min_time_delay; //minium time required time_delay for achieving the wanted speed.
  signed int accel_count; // counter used when computing step_delay for acceleration/decelleration. 
  volatile signed int current_step_position; // contains the current step_position
  unsigned int debug;
 
}speed_profile;
 
 
void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed);
void computation_of_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed);
void init_tipper_position();
void timer1_setup(void);
void output_pin_setup(void);
#endif