4

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?

migrated from electronics.stackexchange.com Oct 10 '16 at 0:43

This question came from our site for electronics and electrical engineering professionals, students, and enthusiasts.

  • What does "the ISR stalls at its last state" mean? Which is the "last state" and how exactly does it "stall"? – brhans Oct 10 '16 at 2:07
  • @brhans It stalls by the global state doesn't go further on from the state DECEL to STOP where the clock source for the timer1 is being removed, and thereby disables the occurrence of the ISR. – Lamda Oct 10 '16 at 9:07
  • 1
    Rewrote the post, which now basically include the full code. – Lamda Oct 10 '16 at 21:52
  • 1
    Can you please stop making multiple posts of what is basically the same issue? You have another question why is function being skipped? where you are speculating that init_tipper_position is being "skipped" or "ignored". The compiler just doesn't ignore functions in a whim. With thread after thread of yours (some now migrated from EE.SE) it is very hard to keep up with you. As soon as someone starts answering you throw all the same code into a new thread with a new idea about what the problem is. – Nick Gammon Oct 11 '16 at 4:44
  • 1
    You should try chatting with Carlton Banks - he seems to have exactly the same issues that you do. – Nick Gammon Oct 11 '16 at 4:45
3

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.

  • The ISR could be changed i agree, It is basically an implementation taken from an Atmel application note. and basically is only one of states performed for each ISR, meaning it might not be that critical. According to the application note atmel.com/images/doc8017.pdf – Lamda Oct 9 '16 at 23:37
  • will it spent 200µs in the Accel state and Decel state, and 35 µs in Run state, approx. measured. I've tried moving things outside the ISR and computed it there, but that often result in different values, or using non updated values even if I use volatile variables or static, which is why i choose to do it as such. the implementation can be found on github – Lamda Oct 9 '16 at 23:37
  • you should consider programming the AVR wit AVR compilers - the Arduino IDE uses avr-g++ which is a perfectly fine tool. It also lets you generate your own interrupt routines if you want to. – Nick Gammon Oct 11 '16 at 4:48
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.

  • The digitalRead and digitalWrite are performed in two different places. When init_tipper_position() is called, is the ISR not activated yet, meaning the Timer1 has been setup to be in (CTC-mode), and a compare match interrupt is being enabled, but at no value has been given to OCR1A. Its only in the next function where it is being given a value. – Lamda Oct 10 '16 at 8:32
  • 2
    There are no “shadow variables” in digitalRead() nor digitalWrite(), and digitalWrite() toggles the pin atomically (i.e. with interrupts disabled), precisely to avoid the kind of reentrancy problems you are afraid of. – Edgar Bonet Oct 10 '16 at 8:32
  • What alternatives do i have for instead of using digitalRead and digitalWrite – Lamda Oct 10 '16 at 8:32
  • 1
    @Lamda: Direct port access, but that won't help unless your problem is genuinely cause by digitalRead() or digitalWrite(). – Edgar Bonet Oct 10 '16 at 8:54
  • I have nothing else to go by. Everything works as it should be if I remove the while(digitalRead()..). I completely agrees that it does not make any sense that this should be the cause. It could maybe a compiler optimisation? as I am currently running low on memory on the board? – Lamda Oct 10 '16 at 9:02
1

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;).

  • I am sure i understand why timer1_setup() starts later due to init_tipper_position is timer1_setup() not executed prior to init_tipper_position? – Lamda Oct 10 '16 at 10:24
  • timer1_setup() is responsible for the initialisation of the timer. But as you i don't touch TCCR1A as it by default is 0. I stop and restart the timer, by removing the clock source, in the State STOP. TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); And global_state becomes false, which makes the code loop in the while(global_state == false) until it becomes true, where it gets a clock source. – Lamda Oct 10 '16 at 10:28
  • Which can be seen in the function compute_speed_profile – Lamda Oct 10 '16 at 10:28
  • @Lamda: Sorry, I meant compute_speed_profile(). I edited the answer accordingly. It is an error not to set TCCR1A. – Edgar Bonet Oct 10 '16 at 10:38
  • I might have been to fast to conclude whether it worked or not. I added TCCR1A in the timer1 setup, and the ISR stalls now in the state STOP... – Lamda Oct 10 '16 at 21:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.