Ask Your Question
0

Control of Stepper motors too slow

asked 2017-08-03 00:54:59 -0500

renanmb gravatar image

updated 2017-08-03 03:15:07 -0500

Thanks for the attention, I need a little help.

The problem is the following, I managed to communicate with my Arduino by rosserial. The thing is that it works, it receives the message that is the PWM frequency for the stepper motors. However, I have a very bad delay. How do I make a Real Time System. Something really fast. My application is controls and it is critical to the reaction be extremely fast.

I am using a library called PWM frequency library . This is the code I am using on my arduino:

#include <Wire.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>

#include <PWM.h> //PWM library for controlling the steppers
//#include <inttypes.h> //not sure about this for the int32_t


ros::NodeHandle  nh;
std_msgs::Int32 str_msg;
ros::Publisher chatter("debug", &str_msg);

//Motor pins
//use pin 11 on the Mega instead, otherwise there is a frequency cap at 31 Hz
const int step_pin1 = 11;     // the pin that define the steps
const int dir_pin1 = 22;      // the dir pin
const int step_pin2 = 12;     // the pin that define the steps
const int dir_pin2 = 24;
int32_t frequency1 = 0; //frequency (in Hz)
int32_t frequency2 = 0; //frequency (in Hz)
int32_t duty1 = 0;
int32_t duty2 = 0;
bool dir1 = true;
bool dir2 = true;

void driveCallback ( const geometry_msgs::Twist&  twistMsg ){
  //need to pubish straigth the motor commands and do the dir calculations on the Arduino
  frequency1 = abs( twistMsg.linear.x ) ;//work as motor1_cmd_callback ---- twistMsg.linear.x must already be converted to int32_t
  frequency2 = abs( twistMsg.linear.y ) ;//work as motor2_cmd_callback ---- twistMsg.linear.y must already be converted to int32_t
  SetPinFrequency(step_pin1, frequency1); //change for the callback function of motor1_cmd
  SetPinFrequency(step_pin2, frequency2); //change for the callback function of motor2_cmd

  str_msg.data = frequency1;
  chatter.publish(&str_msg);
  if(frequency1 > 0){
    duty1 = 32768; 
    if(twistMsg.linear.x >= 0){
    digitalWrite(dir_pin1, HIGH);
    }
    else{
    digitalWrite(dir_pin1, LOW);
    }
  }
  else{
    duty1 = 0;
  }

  if(frequency2 > 0){
    duty2 = 32768;
    if(twistMsg.linear.y >= 0){
    digitalWrite(dir_pin2, HIGH);
    }
    else{
    digitalWrite(dir_pin2, LOW);
    }
  }
  else{
    duty2 = 0;
  }
}

ros::Subscriber<geometry_msgs::Twist> driveSubscriber("comm_drive_robot", &driveCallback) ;

void setup(){
  //initialize all timers except for 0, to save time keeping functions
  InitTimersSafe(); 
  nh.initNode();

  // Subscribe to the steering and throttle messages
  nh.subscribe(driveSubscriber);
  nh.advertise(chatter);

  // =======================================================================================
  /* ================ PWM Function  ======================================================= */
  //PWM + direction to the motors 1
  pinMode(step_pin1, OUTPUT);
  pinMode(dir_pin1, OUTPUT);

  //PWM + direction to the motors 2
  pinMode(step_pin2, OUTPUT);
  pinMode(dir_pin2, OUTPUT);


}

void loop(){
  nh.spinOnce();

  pwmWriteHR(step_pin1, duty1);
  pwmWriteHR(step_pin2, duty2);

  delay(1);
}
edit retag flag offensive close merge delete

Comments

if(twistMsg.linear.x >= 0)

I'm not sure, but geometry_msgs/Twist uses floats/doubles for all its fields. The chance that those are != 0 is rather high, so I don't think this is a good way to do what you want.

And I'm not an expert, but in most cases, the idea is to ..

gvdhoorn gravatar imagegvdhoorn ( 2017-08-03 02:54:40 -0500 )edit

.. compute a PWM duty cycle based on the ratio max/commanded vel. In the case where you have DIR pins, you'll also need to update those (probably based on the sign of the incoming Twist).

But this all depends on whether your motors / controller actually support this mode of operation.

gvdhoorn gravatar imagegvdhoorn ( 2017-08-03 02:57:20 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2017-08-03 09:58:50 -0500

VictorLamoine gravatar image

What is a "big" delay? You should be able to receive a message and execute a simple callback in less than 5 milliseconds, but how big is that?

Suggestions:

  • Remove delay(1)
  • Prefer SPI over I2C as it allows much higher bitrates
  • Use interruptions for your communication; make sure your callback does not exceed the interruption rate! (I don't know how the Arduino ROS library works, it seems to already use interruptions via the Timers)
  • Measure time and analyze what is eating time with Micros()
edit flag offensive delete link more

Comments

I don`t know how to do your step 3. How do I use the AVR interrupts for the communication?

renanmb gravatar imagerenanmb ( 2017-08-03 12:11:51 -0500 )edit

Most of the time you don't have a choice and you use interruptions because the protocol (SPI, I2C) is handled by hardware which triggers interrupts. http://www.gammon.com.au/spi is a very good read.

VictorLamoine gravatar imageVictorLamoine ( 2017-08-04 03:36:05 -0500 )edit

You have to read the ROS Arduino library code to make sure they use standard libraries, I'm pretty sure they do. In that case you are already using interruptions without seeing it explicitly.

VictorLamoine gravatar imageVictorLamoine ( 2017-08-04 03:36:32 -0500 )edit
1

answered 2017-08-06 16:13:39 -0500

renanmb gravatar image

I studied this problem later and found that this is not completely answered. Arduino is a very limited platform and cannot operate at micro seconds level for communicating with the ROS master. It is due the AVR limitations. My project I got a maximum of 30Hz for sending the rotary encoder data and receiving the motors commands. I wish I could go to something closer to the MHz level.

IMUs are also another big issue, I cannot operate a High end IMU, because AVR is too slow.

So for other people also losing connection with the Arduino or having delay happening when communicating with Arduino. Now you know that Arduino is fairly limited and running Timers on the code, and using ROS Throttle is the way to fix.

I would love to hear if someone has some high end PIC or FPGA ready to apply and easy to use as Arduino.

edit flag offensive delete link more

Comments

Would a Raspberry Pi work?

VictorLamoine gravatar imageVictorLamoine ( 2017-08-07 07:32:45 -0500 )edit

I am using a Raspberry Pi as master, but it is actually a terrible idea use it specialized in controlling just the actuator or the sensor. someone recommended me the Nucleo boards, and I also know that PIC and FPGAs are more effective. I will study in detail some solutions and see what is better.

renanmb gravatar imagerenanmb ( 2017-08-07 15:14:20 -0500 )edit

But I know for sure that using the cool stuff only is possible when you buy expensive material or spend a pretty good time developing a good system from low level.

renanmb gravatar imagerenanmb ( 2017-08-07 15:16:56 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-08-03 00:54:59 -0500

Seen: 649 times

Last updated: Aug 06 '17