Robotics StackExchange | Archived questions

Use CMD_Vel to control motors speed for SLAM

Hello

Here is how I get my cmd_vel after running hector slam and create the map on the rviz

roslaunch hector_exploration node exploration_planner.launch
 rosrun hector_exploration_controller simple_exploration_controller 
 topic rostopic echo cmd_vel

I know I need add this to my code to subscribe to the cmd_vel

include "ros.h"

  include "geometry_msgs/Twist.h"

 ros::NodeHandle  nh;

 void velCallback(const geometry_msgs::Twist& vel) {   
     geometry_msgs::Twist new_vel = vel;   
     // HERE I WANT TO USE vel TO DEFINE PWM  }

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

 void loop()
{     
    nh.spinOnce();  
    delay(1); 
}

Here is my Arduino Code for the motor driver

/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(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






//stop();
//delay(250);
}

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);
    }else{
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
    }
    }

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

My question, how do I integrate the cmd_vel and on the motor driver code and use to control the speed of the motors ?

Note : I'm not using encoders

Asked by hamzh.albar@gmail.com on 2018-07-24 22:58:10 UTC

Comments

Answers