Ask Your Question

Help with editing my arduino motor code be controlled by cmd_vel

asked 2018-07-22 01:45:42 -0600 gravatar image

updated 2018-07-22 15:33:28 -0600

Hello everyone,

Im trying to build an autonomous mapping and navigation robot with rplidar, arduino ( controlling the motors and ROS)

So far I I have got lidar, hector slam, hector exploration planner, and run the simple_exploration_controller and of course rosserial working fine.

I'm getting values from cmd_vel from simple_exploration_controller.

My question is how I would change my Arduino code below to have my motors speed controlled by the cmd_vel

Here is my Arduino coder

//motor A connected between A01 and A02
//motor B connected between B01 and B02

int STBY = 10; //standby

//Motor A
int PWMA = 4; //Speed control
int AIN1 = 9; //Direction
int AIN2 = 8; //Direction

//Motor B
int PWMB = 5; //Speed control
int BIN1 = 11; //Direction
int BIN2 = 12; //Direction

void setup(){
pinMode(STBY, OUTPUT);

pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);

pinMode(PWMB, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);

void loop(){
move(1, 255, 1); //motor 1, full speed, left
move(2, 255, 1); //motor 2, full speed, left

//delay(1000); //go for 1 second
//stop(); //stop
//delay(250); //hold for 250ms until move again

//move(1, 128, 0); //motor 1, half speed, right
//move(2, 128, 0); //motor 2, half speed, right



void move(int motor, int speed, int direction){
//Move specific motor at speed and direction
//motor: 0 for B 1 for A
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise

digitalWrite(STBY, HIGH); //disable standby

boolean inPin1 = LOW;
boolean inPin2 = HIGH;

if(direction == 1){
inPin1 = HIGH;

    inPin2 = LOW;

    if(motor == 1){
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);

    void stop(){
    //enable standby
    digitalWrite(STBY, LOW);


, I found this online,

  include "ros.h"

  include "geometry_msgs/Twist.h"

 ros::NodeHandle  nh;

 void velCallback(const geometry_msgs::Twist& vel) {   
     geometry_msgs::Twist new_vel = vel;   

 void setup() {   
     ros::Subscriber<geometry_msgs::Twist>  ("cmd_vel", &velCallback); 

 void loop()

Is that right ? , and should I just change PWM to x in the code ??

edit retag flag offensive close merge delete


Can you provide some info about the hardware? Motors, encoders, motor drivers, etc?

billy gravatar image billy  ( 2018-07-22 02:44:13 -0600 )edit

I'm using Rplidar, 4 motors connected to an Arduino via SparkFun Motor Driver - Dual TB6612FNG as my motor driver. For now I'm not using encoders because as far as I know encoders are not needed for hector slam gravatar image  ( 2018-07-22 02:46:41 -0600 )edit

@billy Any thoughts? gravatar image  ( 2018-07-22 15:18:37 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-07-22 15:29:18 -0600

billy gravatar image

updated 2018-07-23 18:34:48 -0600

The cmd_vel is a velocity command, so you need to convert the velocity command to a PWM setting that corresponds to that speed.

Two scenarios:

1 - In a straight line: reasonably doable. Get a measuring tape and a stop watch and determine speed with both sides set at 25% PWM, then 50%, then 75%, then 100%. Find the equation that converts PWM to speed. (MS excel is good for something like this). This equation may be linear but may be a polynomial or some other function. You'll need to also determine your minimum speed. Start with PWM at 0 and slowly increase it until the robot starts moving. Once it's moving, that is your minimum speed. In Move_base there is a parameter for minimum speed. Not sure about Hector. If the robot doesn't go straight with same PWM value for both sides, you will have different equations for each side.

2 - Turning: This will be very difficult given the way you've setup the robot: 4 wheels and no encoders. 2 wheels and no encoders would be many times less complicated. What makes 4 wheel so difficult is that you could set the PWM different on left and right and the robot may still go straight. Once to you make the PWM difference large enough to break the wheel friction and start rotation, it may rotate very quickly. Basically you need to get all 4 wheels to start sliding to rotate which will require a lot more force than keeping it rotating. If the controllers requests a slow turn, it likely will not turn at all. With only 2 wheels, the wheels do not slide at all when rotating so this effect is much smaller. can try it using the method as above. Use a stop watch and time how long a rotation takes given different values of PMW. You will then also find the minimum rotation speed to use as a parameter in the controller if it is anything like move_base.

With 4 wheels and encoders, the same issue doesn't appear because you know the actual wheel speed.

UPDATED BASED ON FOLLOW ON QUESTION: Sorry if I misunderstood your question.
The bit of code you added is a CALLBACK intended to run when the controller issues a new CMD_VEL message. As you noted, you can paste your own code to convert command to PWM in that location, but that doesn't help you get the PWM values down to the Arduino. I have no experience with ROS and Arduino but I assume you use a serial port to get data to the Arduino. You say ROSSERIAL is working. So ROSSERIAL tutorials should then show you how to connect your PWM values in the CALLBACK to where it gets used in the Arduino.

edit flag offensive delete link more


@billy, thank you, But I'm not sure how to integrate my cmd_vel to my arduino code ? , I updated my question with an update, it would be great if you can show me how to integrate them.

Thank you again gravatar image  ( 2018-07-22 15:34:51 -0600 )edit

see updated answer

billy gravatar image billy  ( 2018-07-23 18:35:00 -0600 )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

1 follower


Asked: 2018-07-22 01:45:42 -0600

Seen: 882 times

Last updated: Jul 23 '18