Drive Benner RMRC
#include <ros.h>
#include <geometry_msgs/Twist.h>
// Motor pins
const int motor1_pwm = 9; // PWM pin for motor 1
const int motor1_dir = 8; // Direction pin for motor 1
const int motor2_pwm = 10; // PWM pin for motor 2
const int motor2_dir = 7; // Direction pin for motor 2
// Encoder pins
const int enc1A = 2; // Encoder 1 channel A
const int enc1B = 3; // Encoder 1 channel B
const int enc2A = 18; // Encoder 2 channel A
const int enc2B = 19; // Encoder 2 channel B
volatile long encoder1_count = 0;
volatile long encoder2_count = 0;
// Encoder ISR
void encoder1ISR() {
if (digitalRead(enc1A) == digitalRead(enc1B)) {
encoder1_count++;
} else {
encoder1_count--;
}
}
void encoder2ISR() {
if (digitalRead(enc2A) == digitalRead(enc2B)) {
encoder2_count++;
} else {
encoder2_count--;
}
}
// ROS NodeHandle
ros::NodeHandle nh;
// ROS subscriber callback
void cmdVelCallback(const geometry_msgs::Twist &cmd) {
// Map linear and angular velocity to motor commands
float left_motor_speed = cmd.linear.x - cmd.angular.z;
float right_motor_speed = cmd.linear.x + cmd.angular.z;
// Set motor directions and speeds
analogWrite(motor1_pwm, abs(left_motor_speed) * 255);
digitalWrite(motor1_dir, left_motor_speed >= 0 ? HIGH : LOW);
analogWrite(motor2_pwm, abs(right_motor_speed) * 255);
digitalWrite(motor2_dir, right_motor_speed >= 0 ? HIGH : LOW);
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", cmdVelCallback);
void setup() {
// Motor pins setup
pinMode(motor1_pwm, OUTPUT);
pinMode(motor1_dir, OUTPUT);
pinMode(motor2_pwm, OUTPUT);
pinMode(motor2_dir, OUTPUT);
// Encoder pins setup
pinMode(enc1A, INPUT_PULLUP);
pinMode(enc1B, INPUT_PULLUP);
pinMode(enc2A, INPUT_PULLUP);
pinMode(enc2B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(enc1A), encoder1ISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(enc2A), encoder2ISR, CHANGE);
// Initialize ROS
nh.initNode();
nh.subscribe(cmd_vel_sub);
}
void loop() {
nh.spinOnce();
delay(10);
}
#include <ros.h>
#include <geometry_msgs/Twist.h>
// Motor pins
const int motor1_pwm = 9; // PWM pin for motor 1
const int motor1_dir = 8; // Direction pin for motor 1
const int motor2_pwm = 10; // PWM pin for motor 2
const int motor2_dir = 7; // Direction pin for motor 2
// Encoder pins
const int enc1A = 2; // Encoder 1 channel A
const int enc1B = 3; // Encoder 1 channel B
const int enc2A = 18; // Encoder 2 channel A
const int enc2B = 19; // Encoder 2 channel B
volatile long encoder1_count = 0;
volatile long encoder2_count = 0;
// Encoder ISR
void encoder1ISR() {
if (digitalRead(enc1A) == digitalRead(enc1B)) {
encoder1_count++;
} else {
encoder1_count--;
}
}
void encoder2ISR() {
if (digitalRead(enc2A) == digitalRead(enc2B)) {
encoder2_count++;
} else {
encoder2_count--;
}
}
// ROS NodeHandle
ros::NodeHandle nh;
// ROS subscriber callback
void cmdVelCallback(const geometry_msgs::Twist &cmd) {
// Map linear and angular velocity to motor commands
float left_motor_speed = cmd.linear.x - cmd.angular.z;
float right_motor_speed = cmd.linear.x + cmd.angular.z;
// Set motor directions and speeds
analogWrite(motor1_pwm, abs(left_motor_speed) * 255);
digitalWrite(motor1_dir, left_motor_speed >= 0 ? HIGH : LOW);
analogWrite(motor2_pwm, abs(right_motor_speed) * 255);
digitalWrite(motor2_dir, right_motor_speed >= 0 ? HIGH : LOW);
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", cmdVelCallback);
void setup() {
// Motor pins setup
pinMode(motor1_pwm, OUTPUT);
pinMode(motor1_dir, OUTPUT);
pinMode(motor2_pwm, OUTPUT);
pinMode(motor2_dir, OUTPUT);
// Encoder pins setup
pinMode(enc1A, INPUT_PULLUP);
pinMode(enc1B, INPUT_PULLUP);
pinMode(enc2A, INPUT_PULLUP);
pinMode(enc2B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(enc1A), encoder1ISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(enc2A), encoder2ISR, CHANGE);
// Initialize ROS
nh.initNode();
nh.subscribe(cmd_vel_sub);
}
void loop() {
nh.spinOnce();
delay(10);
}
#include <ros.h>
#include <geometry_msgs/Twist.h>
// Motor pins
const int motor1_pwm = 9; // PWM pin for motor 1
const int motor1_dir = 8; // Direction pin for motor 1
const int motor2_pwm = 10; // PWM pin for motor 2
const int motor2_dir = 7; // Direction pin for motor 2
// Encoder pins
const int enc1A = 2; // Encoder 1 channel A
const int enc1B = 3; // Encoder 1 channel B
const int enc2A = 18; // Encoder 2 channel A
const int enc2B = 19; // Encoder 2 channel B
volatile long encoder1_count = 0;
volatile long encoder2_count = 0;
// Encoder ISR
void encoder1ISR() {
if (digitalRead(enc1A) == digitalRead(enc1B)) {
encoder1_count++;
} else {
encoder1_count--;
}
}
void encoder2ISR() {
if (digitalRead(enc2A) == digitalRead(enc2B)) {
encoder2_count++;
} else {
encoder2_count--;
}
}
// ROS NodeHandle
ros::NodeHandle nh;
// ROS subscriber callback
void cmdVelCallback(const geometry_msgs::Twist &cmd) {
// Map linear and angular velocity to motor commands
float left_motor_speed = cmd.linear.x - cmd.angular.z;
float right_motor_speed = cmd.linear.x + cmd.angular.z;
// Set motor directions and speeds
analogWrite(motor1_pwm, abs(left_motor_speed) * 255);
digitalWrite(motor1_dir, left_motor_speed >= 0 ? HIGH : LOW);
analogWrite(motor2_pwm, abs(right_motor_speed) * 255);
digitalWrite(motor2_dir, right_motor_speed >= 0 ? HIGH : LOW);
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", cmdVelCallback);
void setup() {
// Motor pins setup
pinMode(motor1_pwm, OUTPUT);
pinMode(motor1_dir, OUTPUT);
pinMode(motor2_pwm, OUTPUT);
pinMode(motor2_dir, OUTPUT);
// Encoder pins setup
pinMode(enc1A, INPUT_PULLUP);
pinMode(enc1B, INPUT_PULLUP);
pinMode(enc2A, INPUT_PULLUP);
pinMode(enc2B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(enc1A), encoder1ISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(enc2A), encoder2ISR, CHANGE);
// Initialize ROS
nh.initNode();
nh.subscribe(cmd_vel_sub);
}
void loop() {
nh.spinOnce();
delay(10);
}