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