ROS LiDAR Robot - Mark1

/**
 * @file         mbot_ranger_cmd_vel.ino
 * @author       Erwin Rabano Pasia
 * @version      V1.0
 * @date         2022/02/01
 * @description  This file is the cmd_vel code for the mBot Ranger.
 * Using the Makeblock Me Auriga Board which is actually an integrated MCU (Arduino Mega2560 and an on-board TB6612 Motor Driver) 
 * for the Two Me-180 Encoder Motors.
 */


#include <ros.h>
#include <geometry_msgs/Twist.h>

// Maximum PWM = 255
int maxPwm=225;
int halfPwm=225;
int stopPwm=0;
int turnPwm=255;

//Encoder Motor Left
const int pwmMotor1 = 11;
const int inMotor1_1 = 49;
const int inMotor1_2 = 48;

//Encoder Motor Right
const int pwmMotor2 = 10;
const int inMotor2_1 = 47;
const int inMotor2_2 = 46;


ros::NodeHandle  nh;

void MoveBack() {
    digitalWrite(inMotor1_1, HIGH);
    digitalWrite(inMotor1_2, LOW);
    analogWrite(pwmMotor1,maxPwm);
    digitalWrite(inMotor2_1, LOW);
    digitalWrite(inMotor2_2, HIGH);
    analogWrite(pwmMotor2,maxPwm);
    Serial.println("Move Backward at Full speed"); //For debugging
}

void MoveStop() {
    digitalWrite(inMotor1_1, LOW);
    digitalWrite(inMotor1_2, LOW);
    analogWrite(pwmMotor1,stopPwm);
    digitalWrite(inMotor2_1, LOW);
    digitalWrite(inMotor2_2, LOW);
    analogWrite(pwmMotor2,stopPwm);
    Serial.println("At Full-Stop Mode"); //For debugging
}
void MoveRight() {
    digitalWrite(inMotor1_1, LOW);
    digitalWrite(inMotor1_2, HIGH);
    analogWrite(pwmMotor1,turnPwm);//Set speed via PWM
    digitalWrite(inMotor2_1, LOW);
    digitalWrite(inMotor2_2, HIGH);
    analogWrite(pwmMotor2,turnPwm);//Set speed via PWM
    Serial.println("Turning the Robot Right"); //For debugging
}
void MoveLeft() {
    digitalWrite(inMotor1_1, HIGH);
    digitalWrite(inMotor1_2, LOW);
    analogWrite(pwmMotor1,turnPwm);//Set speed via PWM
    digitalWrite(inMotor2_1, HIGH);
    digitalWrite(inMotor2_2, LOW);
    analogWrite(pwmMotor2,turnPwm);//Set speed via PWM
    Serial.println("Turning the Robot Left"); //For debugging
}
void MoveFwd() {
    digitalWrite(inMotor1_1, LOW);
    digitalWrite(inMotor1_2, HIGH);
    analogWrite(pwmMotor1,maxPwm);
    digitalWrite(inMotor2_1, HIGH);
    digitalWrite(inMotor2_2, LOW);
    analogWrite(pwmMotor2,maxPwm);
    Serial.println("Move Forward at Full speed"); //For debugging
}

void cmd_vel_cb(const geometry_msgs::Twist & msg) {
  // Read the message.
  // We only care about the linear x, and the rotational z.
  const float x = msg.linear.x;
  const float z_rotation = msg.angular.z;

  // Decide on the encoder motor state we need, according to command.
  if (x > 0 && z_rotation == 0) {
    MoveFwd();
  }
  else if (x == 0 && z_rotation == 1) {
    MoveRight();
  }
else if (x == 0 && z_rotation < 0) {
    MoveLeft();
  }
else if (x < 0 && z_rotation == 0) {
    MoveBack();
  }
else{
    MoveStop();
  }
}
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", cmd_vel_cb);
void setup() {
  // pinMode(right_back, OUTPUT);    
  // pinMode(left_back, OUTPUT);
  // pinMode(right_front, OUTPUT);
  // pinMode(left_front, OUTPUT);

  pinMode(pwmMotor1, OUTPUT);
  pinMode(pwmMotor2, OUTPUT);
  
  nh.initNode();
  nh.subscribe(sub);
}

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