Help with editing my arduino motor code be controlled by cmd_vel
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
//delay(1000);
//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);
}
Update
, 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;
// 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);
}
Is that right ? , and should I just change PWM to x in the code ??
Can you provide some info about the hardware? Motors, encoders, motor drivers, etc?
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
@billy Any thoughts?