why does function affect my ISR?

Question

I am currently running this piece of code:

main.ino

#include "speed_profile.h"


void setup() {
  // put your setup code here, to run once:
  output_pin_setup();
  cli();
  timer1_setup();
  sei();
}

void loop() 
{
      //int motor_steps = 23400;//-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
      init_tipper_position();
      compute_speed_profile(23400, 500, 500, 1000); 
}

Speed_profile.cpp

#include "speed_profile.h"


volatile speed_profile profile;

ros::NodeHandle_<ArduinoHardware, 1, 2, 125, 125> nh;
//ros::NodeHandle 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);
  //nh.subscribe(sub_command);
  profile.current_step_position = 0;
  delay(10);
  nh.getHardware()->setBaud(57600);
}

void init_tipper_position()
{
  //digitalWrite(en_pin, HIGH);
  //delay(1);

  //digitalWrite(dir_pin, LOW);
  //delay(1);


  while(!(PINB & (1 << 2)))  
  {
    //PORTB ^= _BV(PB3);
    //delayMicroseconds(100);
  }

  //digitalWrite(en_pin, LOW);

}
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;
    //profile.step_counter = steps_to_speed;
  }
}

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 = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
    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 (motor_steps < 0)
  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));


  while (global_state == true)
  { 
    status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
    status_tipper_availability.data = profile.run_state;

    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:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      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:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      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:
      //digitalWrite(step_pin, !digitalRead(step_pin));
      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)
      {
        digitalWrite(en_pin, !digitalRead(en_pin));
        //PORTB &= ~_BV(PB3);
        profile.run_state = STOP;
      }
      break;

  }

  profile.first_step_delay = new_first_step_delay;

}

And the Speed_profile.h

/*
 * 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

}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

The problems is with the ISR. The function init_tipper_position seem to be causing a timing problem that makes is impossible the ISR to go through all its states. I changed some parts of the code based on @EdgarBonet 's answer, which seem have helped a bit. The problem here is that the program stalls when the it reaches the state STOP. This is not a issue at all if the prior function is executed..?? why am I having this issue?

Answer 1

This isn't the sort of approach you should use in an ISR. You need to get into the ISR, and get out of it ASAP. Set a flag, and read the flag in your code to decide what needs to be done. There should be very little in the way of decisions and branching in an ISR. Most certainly, there should be zero floating point math in your ISR, especially on a platform that doesn't have a floating point processor (actually, my preference is to avoid floating point math wherever possible in an embedded system, but I might be a bit extreme.

Start from scratch, rebuilding your ISR to do only what needs to be done, and use main to do the rest of it (or "loop" or some such on an Arduino).

Having spent no real time on an Arduino, interrupts just don't seem all that convenient to use to me. If the Arduino environment doesn't let you use interrupts the right way, you should consider programming the AVR wit AVR compilers.

Answer 2

I'm not very familiar with the details of the Arduino infrastructure either, but I think your fundamental problem is the fact that you are doing GPIO (i.e., digitalRead() and digitalWrite()) both in your ISR and in your main loop().

Since these functions manipulate global hardware and use various "shadow" variables to keep track of their state, it's very unlikely that they are "reentrant". A reentrant function is written without global/static variables (and a few other restrictions), such that one call to it can be interrupted and another call made to it without interfering with the first one.

The Arduino environment was never intended for projects in which issues such as reentrancy should arise; such projects should be using using a "real" RTOS or equivalent. Yes, you can shoehorn such project into the Arduino environment, but only if you already know exactly what you're doing.

Answer 3

I narrowed the search space to be inside the init_tipper_position

I see nothing wrong with this function, except that it takes some time to execute, and makes compute_speed_profile() start later. So I am guessing yours is a timing problem: your code may be sensitive to the value held by the timer when compute_speed_profile() starts. It is just a wild guess, as you did not show all your code, and the real problem may be somewhere we cannot see.

Did you completely initialize Timer 1 before using it? I see no mention of TCCR1A in the part you showed us. You should be aware that the Arduino core library initializes this timer to provide 8-bit PWM. It is likely that you will need to undo that initialization, e.g., at the very beginning of your program:

TCCR1A = 0;  // undo the initialization done by...
TCCR1B = 0;  // ...the Arduino core library
TCNT1  = 0;  // reset the timer

This is the default state set by the hardware at boot, and the Atmel application note you are using as a reference may well assume this state has not been tampered with.

Also, I did not check your algorithm, but since your are stopping and restarting the timer, it may make sense to also reset it (TCNT1 = 0;).