Arduino omnidirectional ROS driver

kolinhahd · February 18, 2023, 10:41pm

im building an ROS omnidirectional platform and only using the arduino to compute the cmd_vel messages and to run based on them, the thing is that the code is not working, i dont know if i am not setting up badly the rosserial or whats going on but the robot is not moving with a simple teleop script, i need help with anyone that has knowledge on this

the code is the following:

#define enablePin 8
#include <Stepper.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>

//Medidas
const double WHEEL_RADIUS = 0.04; // RAIO DA RODA;
const double WHEEL_SEPARATION_WIDTH = 0.10;  // Separação da roda até ao centro na Horizontal
const double WHEEL_SEPERATION_LENGTH = 0.125; // Separação da roda até ao centro na vertical
const double WHEEL_GEOMETRY = WHEEL_SEPARATION_WIDTH + WHEEL_SEPERATION_LENGTH;

//Parâmetros do Motor
const int STEPS_PER_REVOLUTION = 200; // Numero de passos por cada volta do motor
const int MOTOR_SPEED = 60; // Não me perguntem que eu também não sei


//Iniciar suporte para os nós do ROS
ros::NodeHandle nh;



Stepper RODAESQFrente(STEPS_PER_REVOLUTION, 2, 5);
Stepper RODAESQTraseira(STEPS_PER_REVOLUTION, 3, 6);
Stepper RODADRTFrente(STEPS_PER_REVOLUTION, 4, 7);
Stepper RODADRTTraseira(STEPS_PER_REVOLUTION, 12, 13);



geometry_msgs::Twist cmd_vel_msg;
ros::Subscriber<geometry_msgs::Twist> sub_cmd_vel("cmd_vel", &cmd_vel_callback);

void setup()
{
  nh.InitNode();
  nh.subscribe(sub_cmd_vel);
  RODAESQFrente.setSpeed(MOTOR_SPEED);
  RODAESQTraseira.setSpeed(MOTOR_SPEED);
  RODADRTFrente.setSpeed(MOTOR_SPEED);
  RODADRTTraseira.setSpeed(MOTOR_SPEED);
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin, LOW);
}

void cmd_vel_callback(const geometry_msgs::Twist& msg) {
  cmd_vel_msg = msg;

  double linear_x = msg.linear.x;
  double linear_y = msg.linear.y;
  double angular_z = msg.angular.z;


  double wFE_velocity =  (linear_x - linear_y - (WHEEL_GEOMETRY) * angular_z);
  double wFD_velocity =  (linear_x + linear_y + (WHEEL_GEOMETRY) * angular_z);
  double wTE_velocity =  (linear_x + linear_y - (WHEEL_GEOMETRY) * angular_z);
  double wTD_velocity =  (linear_x - linear_y + (WHEEL_GEOMETRY) * angular_z);

  // Convert rad/s to steps/s for each motor using the kinematics equations
  double FE_steps = wFE_velocity * STEPS_PER_REVOLUTION / (2 * PI * WHEEL_RADIUS);
  double FD_steps = wFD_velocity * STEPS_PER_REVOLUTION / (2 * PI * WHEEL_RADIUS);
  double TE_steps = wTE_velocity * STEPS_PER_REVOLUTION / (2 * PI * WHEEL_RADIUS);
  double TD_steps = wTD_velocity * STEPS_PER_REVOLUTION / (2 * PI * WHEEL_RADIUS);

  double FE_mps = (FE_steps * 0.0314) * WHEEL_RADIUS;
  double FD_mps = (FD_steps * 0.0314) * WHEEL_RADIUS;
  double TE_mps = (TE_steps * 0.0314) * WHEEL_RADIUS;
  double TD_mps = (TD_steps * 0.0314) * WHEEL_RADIUS;


/*FE = Front left wheel
  Serial.print("Wheel FE: ");
  Serial.print(wFE_velocity);
  Serial.print("rad/s ");
  Serial.print(FE_steps);
  Serial.print(" steps/s ");
  Serial.print(FE_ms);
  Serial.println(" m/s ");*/
  
/*FD = Front right wheel  
  Serial.print("Wheel FE: ");
  Serial.print(wFD_velocity);
  Serial.print("rad/s ");
  Serial.print(FD_steps);
  Serial.print(" steps/s ");
  Serial.print(FD_ms);
  Serial.println(" m/s ");*/
/*TE = Rear left wheel  
  Serial.print("Wheel FE: ");
  Serial.print(wTE_velocity);
  Serial.print("rad/s ");
  Serial.print(TE_steps);
  Serial.print(" steps/s ");
  Serial.print(TE_ms);
  Serial.println(" m/s ");*/
/*TD = Rear right wheel
  Serial.print("Wheel FE: ");
  Serial.print(wTD_velocity);
  Serial.print("rad/s ");
  Serial.print(TD_steps);
  Serial.print(" steps/s ");
  Serial.print(TD_ms);
  Serial.println(" m/s ");*/

  
  RODAESQFrente.step(FE_steps);
  RODAESQTraseira.step(FD_steps);
  RODADRTFrente.step(TE_steps);
  RODADRTTraseira.step(TD_steps);
}



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

i also used an example as base but couldn't understand much of it which is the following:


#include <ros.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Twist.h>
#include <Stepper.h>



ros::NodeHandle nh;

// Constants for wheel radius and wheel separation distance
const double wheel_radius = 0.1; // 10 cm
const double wheel_separation = 0.3; // 30 cm

// Constants for number of steps per revolution and speed of each motor
const int motor_steps = 200;
const int motor_speed = 60;

// Stepper objects for each motor
Stepper motor1(motor_steps, 8, 10, 9, 11);
Stepper motor2(motor_steps, 13, 11, 12, 14);
Stepper motor3(motor_steps, 2, 3, 4, 5);
Stepper motor4(motor_steps, 7, 6, 5, 8);

std_msgs::Int32 motor1_steps_msg;
ros::Subscriber<std_msgs::Int32> sub_motor1_steps("motor1_steps", &motor1_steps_callback);

std_msgs::Int32 motor2_steps_msg;
ros::Subscriber<std_msgs::Int32> sub_motor2_steps("motor2_steps", &motor2_steps_callback);

std_msgs::Int32 motor3_steps_msg;
ros::Subscriber<std_msgs::Int32> sub_motor3_steps("motor3_steps", &motor3_steps_callback);

std_msgs::Int32 motor4_steps_msg;
ros::Subscriber<std_msgs::Int32> sub_motor4_steps("motor4_steps", &motor4_steps_callback);

geometry_msgs::Twist cmd_vel_msg;
ros::Subscriber<geometry_msgs::Twist> sub_cmd_vel("cmd_vel", &cmd_vel_callback);

nav_msgs::Odometry odom_msg;
ros::Publisher odom_pub("odom", &odom_msg);

// Callback functions for motor step count messages
void motor1_steps_callback(const std_msgs::Int32& msg) {
  motor1_steps_msg = msg;
}

void motor2_steps_callback(const std_msgs::Int32& msg) {
motor2_steps_msg = msg;
}

void motor3_steps_callback(const std_msgs::Int32& msg) {
motor3_steps_msg = msg;
}

void motor4_steps_callback(const std_msgs::Int32& msg) {
motor4_steps_msg = msg;
}

// Callback function for cmd_vel message
void cmd_vel_callback(const geometry_msgs::Twist& msg) {
cmd_vel_msg = msg;

double linear_velocity = msg.linear.x;
double angular_velocity = msg.angular.z;

double left_wheel_velocity = linear_velocity - (wheel_separation / 2) * angular_velocity;
double right_wheel_velocity = linear_velocity + (wheel_separation / 2) * angular_velocity;

double motor1_velocity = (left_wheel_velocity + right_wheel_velocity) / 2;
double motor2_velocity = (left_wheel_velocity + right_wheel_velocity) / 2;
double motor3_velocity = (left_wheel_velocity - right_wheel_velocity) / 2;
double motor4_velocity = (left_wheel_velocity - right_wheel_velocity) / 2;

int motor1_steps = motor1_velocity * motor_steps / (2 * wheel_radius * M_PI);
int motor2_steps = motor2_velocity * motor_steps / (2 * wheel_radius * M_PI);
int motor3_steps = motor3_velocity * motor_steps / (2 * wheel_radius * M_PI);
int motor4_steps = motor4_velocity * motor_steps / (2 * wheel_radius * M_PI);

motor1.step(motor1_steps);
motor2.step(motor2_steps);
motor3.step(motor3_steps);
motor4.step(motor4_steps);

}

void setup() {
// Initialize the ROS node and subscribers
nh.initNode();
nh.subscribe(sub_motor1_steps);
nh.subscribe(sub_motor2_steps);
nh.subscribe(sub_motor3_steps);
nh.subscribe(sub_motor4_steps);
nh.subscribe(sub_cmd_vel);


// Initialize the odometry publisher
odom_pub.advertise();

// Set the speed of each motor
motor1.setSpeed(motor_speed);
motor2.setSpeed(motor_speed);
motor3.setSpeed(motor_speed);
motor4.setSpeed(motor_speed);
}

void loop() {
void loop() {
  // Check if there are new messages from the motor step count subscribers
  nh.spinOnce();

  // Compute the new wheel speeds based on the motor step counts
  double d1 = wheel_radius * (2.0 * M_PI / motor_steps) * motor1_steps_msg.data;
  double d2 = wheel_radius * (2.0 * M_PI / motor_steps) * motor2_steps_msg.data;
  double d3 = wheel_radius * (2.0 * M_PI / motor_steps) * motor3_steps_msg.data;
  double d4 = wheel_radius * (2.0 * M_PI / motor_steps) * motor4_steps_msg.data;

  // Compute the new linear and angular velocities based on the wheel speeds
  double linear_velocity = (d1 + d2 + d3 + d4) / 4.0;
  double angular_velocity = (-d1 + d2 + d3 - d4) / (4.0 * wheel_separation);
  
  // Reset the motor step counts
  motor1_steps_msg.data = 0;
  motor2_steps_msg.data = 0;
  motor3_steps_msg.data = 0;
  motor4_steps_msg.data = 0;

  // Control the motor speeds based on the new linear and angular velocities
  double v1 = linear_velocity - angular_velocity * wheel_separation / 2.0;
  double v2 = linear_velocity + angular_velocity * wheel_separation / 2.0;
  double v3 = linear_velocity - angular_velocity * wheel_separation / 2.0;
  double v4 = linear_velocity + angular_velocity * wheel_separation / 2.0;

  int steps_per_second_1 = (int)(v1 / (wheel_radius * 2.0 * M_PI) * motor_steps);
  int steps_per_second_2 = (int)(v2 / (wheel_radius * 2.0 * M_PI) * motor_steps);
  int steps_per_second_3 = (int)(v3 / (wheel_radius * 2.0 * M_PI) * motor_steps);
  int steps_per_second_4 = (int)(v4 / (wheel_radius * 2.0 * M_PI) * motor_steps);

  motor1.setSpeed(steps_per_second_1);
  motor2.setSpeed(steps_per_second_2);
  motor3.setSpeed(steps_per_second_3);
  motor4.setSpeed(steps_per_second_4);

  motor1.step(motor1_steps_msg.data);
  motor2.step(motor2_steps_msg.data);
  motor3.step(motor3_steps_msg.data);
  motor4.step(motor4_steps_msg.data);

// Wait for the next iteration
delay(100);
}

Anyone one that can tell me whats wrong with my code please tell me

Railroader · February 18, 2023, 11:34pm

Those words contain no useful information. All code always "work", does something. The question is what it does.
Start using debug printouts unless You have a logic analyser to use.

system · August 17, 2023, 11:34pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Need help with code, using rosserial in motor control Programming	3	626	April 1, 2024
Building a "cheap" ROS + Arduino Turtlebot. Need help with code/hardware Robotics	16	6820	October 17, 2021
Inverse Kinematics of Differential Drive Robots and Odometry Programming	58	5657	December 31, 2022
4WD Omni wheel based autonomous robot General Guidance	4	1053	March 31, 2017
Odometry does not transform in rviz General Guidance	6	498	October 22, 2022

Arduino omnidirectional ROS driver

Related topics