0

I am at the moment trying via ros to publish a status message about how many steps a motor has moved. Problem, is that value doesn't to be readable, or the only thing I am able to read from the topic is a zero.

The value i want to publish is being incremented inside a ISR routine, so i have a single thread that continuesly publishes the messages, and then when an ISR is called, increment it the value.

I tested with serial to see what might have been wrong, but the output of the serial doesn't seem to make any sence, isn't readable.. Is outputs alot of "??" eventhoug I am using the right baudrate and so on. So what is the problem here?

speed_profile.cpp

#include "speed_profile.h"


volatile speed_profile profile;
ros::NodeHandle nh;

std_msgs::Int16 status_step_count;
ros::Publisher chatter("tipper_status", &status_step_count);

void output_pin_setup()
{
  pinMode(en_pin,OUTPUT);
  pinMode(dir_pin,OUTPUT);
  pinMode(step_pin,OUTPUT); 
  nh.initNode();
  nh.advertise(chatter);
}

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)
{
  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;
    Serial.print("profile.moved_steps: ");
    Serial.println(profile.moved_steps);
    Serial.print('\n');
    sei();
    //chatter.publish( &status_step_count);
    //nh.spinOnce();
    delay(1);
  }

}



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;
      rest = 0;
      TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); // Stop the timer,  No clock source
      break;

    case ACCEL:
      digitalWrite(step_pin,!digitalRead(step_pin));
      //delay(1);
      //digitalWrite(step_pin,LOW);
      step_count++;
      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));

      //delay(1);
      //digitalWrite(step_pin,LOW);
      step_count++;
      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));      
      step_count++;
      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
      //digitalWrite(step_pin,!digitalRead(step_pin));
      if(profile.accel_count >= 0)
      {
        digitalWrite(en_pin,!digitalRead(en_pin));
        profile.run_state = STOP;
      }

      break;

  }
    profile.first_step_delay = new_first_step_delay;  
    profile.moved_steps = step_count;  
}

speed_profile.h

#ifndef speed_profile_h
#define speed_profile_h


#include <Arduino.h> 
#include <ros.h>
#include <std_msgs/Int16.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

// Motor direction 
#define CW  0
#define CCW 1

typedef struct 
{
 volatile unsigned char run_state : 3; // Determining the state of the speed profile
 volatile unsigned char dir: 1; // Determining the direction the motor has to move - Start being CCW 
 volatile unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate. 
 volatile unsigned int decel_start; //  Determines at which step the deceleration should begin. 
 volatile signed int decel_length; // Set the deceleration length
 volatile signed int min_time_delay; //minium time delay c0
 volatile signed int accel_count; // counter used when computing step_delay for acceleration/decelleration. 
 volatile unsigned int moved_steps; // Used by ros to publish current tipper position

}speed_profile;


void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed);
void timer1_setup(void);
void output_pin_setup(void);
#endif

main.ino

#include "speed_profile.h"



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

void loop() 
{
  // put your main code here, to run repeatedly:
    Serial.println("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");

    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)
    {

    }
}
4
  • Could you add least add the entire code at the end. I'm unable to decipher what is going on with just the snippets you posted. Could you also add some sample output of the serial?
    – Gerben
    Commented Oct 6, 2016 at 18:52
  • I just found out that it works with an baudrate of 57600, eventhough I initiliaze it to 9600.
    – Lamda
    Commented Oct 6, 2016 at 19:04
  • full code added.
    – Lamda
    Commented Oct 6, 2016 at 19:08
  • What Arduino do you have, and what clock frequency is it running at?
    – Nick Gammon
    Commented Oct 7, 2016 at 0:51

3 Answers 3

2

This is highly inadvisable:

cli();
status_step_count.data = profile.moved_steps;
Serial.print("profile.moved_steps: ");
Serial.println(profile.moved_steps);
Serial.print('\n');
sei();

Doing serial prints with interrupts off could hang your sketch because the Serial output routines require interrupts on to work.

Later versions of the IDE might not hang completely but will block until the data can be written.

1

The problem may be due to something you haven't told us about, so the following suggestions are just general ideas.

First, try out whether serial communication works ok at 230400 when your stepper code isn't running. For example, add some debug lines to skip over whatever code turns on interrupts to run the stepper, and see if serial communication is ok.

If serial communication still fails with stepper interrupts off, try lower serial rates, eg 19200 or 9600. In addition, in your Publisher add a reasonable delay, eg half a second or a second, between serial writes. You might also not issue a write whenever the total steps has changed less than a hundred counts, or some similar number.

I'd probably factor toggling step_pin out of the switch, via code like the following.

if (profile.run_state != STOP) {
   // Toggle step_pin by writing 1 to its bit in PINB
   PINB = step_pin_mask; // pin 11 = PB3, so mask is 1<<PB3, ie, 1<<3
   step_count++;
}
switch (profile.run_state) {
   ...

Using digitalRead() and digitalWrite() adds a fair amount of overhead, undesirable in an ISR. (See their source code at garretlab links, digitalRead() and digitalWrite()) That extra overhead could possibly account for glitches in serial communications.

By contrast, writing a mask bit to an input port is about the fastest way for software to toggle a bit or set of bits. It is, of course, less flexible and less obvious.

1
  • I am not sure what else would be missing than the complete code.. but thanks for the pointers with make it faster.. it is a lot of code, and I am doing a sqrt() operation somewhere, so it would be very use full to compensate for that.
    – Lamda
    Commented Oct 6, 2016 at 19:47
0

That is QUITE a high baud rate. You might want to lower it. I know that Arduinos running at 8MHZ can barely handle 112500. Do not push it too far. I do not know what Arduino you are using but at 16MHZ, don't push it too far. Also make sure that at that baud rate you supply a lot of juice to your Arduino so that it can run more smoothly.

2
  • i tried with 9600 and same result
    – Lamda
    Commented Oct 6, 2016 at 16:44
  • Sorry I couldn't help.
    – Dat Ha
    Commented Oct 6, 2016 at 16:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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