Rosanswers logo

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 ??

Originally posted by [email protected] on ROS Answers with karma: 35 on 2018-07-22

Post score: 1

Original comments

Comment by billy on 2018-07-22:
Can you provide some info about the hardware? Motors, encoders, motor drivers, etc?

Comment by [email protected] on 2018-07-22:
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

Comment by [email protected] on 2018-07-22:
@billy Any thoughts?


1 Answer 1


Rosanswers logo

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. But...you 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.

Originally posted by billy with karma: 1850 on 2018-07-22

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by [email protected] on 2018-07-22:
@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

Comment by billy on 2018-07-23:
see updated answer


Your Answer

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