Hi guys, I hope this message finds you well.
I am currently working on a project involving a car that detects and avoids obstacles using ROS (Robot Operating System) commands via Bluetooth. I need assistance with integrating the servo motor programming so that when the robot detects an obstacle, it will stop, and the servo motor will turn left and right to choose the best path. pls help me
this my code:
#include <ros.h>
#include <std_msgs/Int32.h>
int m1a = 4;
int m1b = 5;
int m2a = 6;
int m2b = 7;
int speedPinRight = 2;
int speedPinLeft = 3;
int demar = 0;
#define trigPin A1
#define echoPin A2
ros::NodeHandle nh;
void callBackFunctionEtat(const std_msgs::Int32 &EtatROS) {
int statue = EtatROS.data;
//////////////////////////////////////////////////////////////////////////////////////////////commandes
if(statue == 2)
{
Backward();
demar = 1;
}
else if(statue == 3)
{
turnLeft();
demar = 1;
}
else if(statue == 1)
{
Forward();
demar = 1;
}
else if(statue == 4)
{
turnRight();
demar = 1;
}
else if(statue == 0)
{
stop();
demar = 1;
}
//////////////////////////////////////////////////////////////////////////////////////////////
}
std_msgs::Int32 CapteurROS;
ros::Publisher CapteurROSPublisher("Capteur", &CapteurROS);
ros::Subscriber<std_msgs::Int32> EtatROSsubscriber("Etat", &callBackFunctionEtat);
void setup() {
nh.getHardware()->setBaud(9600);
nh.initNode();
nh.subscribe(EtatROSsubscriber);
nh.advertise(CapteurROSPublisher);
//////////////////////////////////////////////////////////////////////////////////////////////OUTPUT INPUT
pinMode(speedPinRight, OUTPUT);
pinMode(speedPinLeft, OUTPUT);
pinMode(m1a, OUTPUT);
pinMode(m1b, OUTPUT);
pinMode(m2a, OUTPUT);
pinMode(m2b, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
//////////////////////////////////////////////////////////////////////////////////////////////
}
void loop() {
nh.spinOnce();
delay(5);
//////////////////////////////////////////////////////////////////////////////////////////////
if (demar == 1 ) {
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance < 15) {
CapteurROS.data = 1 ;
}
else {
CapteurROS.data = 0 ;
}
CapteurROSPublisher.publish(&CapteurROS);
}
//////////////////////////////////////////////////////////////////////////////////////////////
}
void Backward(){
digitalWrite(m1a, LOW);
digitalWrite(m2a, LOW);
digitalWrite(m1b, HIGH);
digitalWrite(m2b, HIGH);
analogWrite(speedPinRight, 150);
analogWrite(speedPinLeft, 150);
}
void Forward(){
digitalWrite(m1a, HIGH);
digitalWrite(m2a, HIGH);
digitalWrite(m1b, LOW);
digitalWrite(m2b, LOW);
analogWrite(speedPinRight, 150);
analogWrite(speedPinLeft, 150);
}
void turnLeft(){
digitalWrite(m1a, LOW);
digitalWrite(m2a, LOW);
digitalWrite(m1b, HIGH);
digitalWrite(m2b, LOW);
analogWrite(speedPinLeft, 200);
}
void turnRight(){
digitalWrite(m1a, LOW);
digitalWrite(m2a, HIGH);
digitalWrite(m1b, LOW);
digitalWrite(m2b, LOW);
analogWrite(speedPinRight, 150);
}
void stop(){
digitalWrite(m1a, LOW);
digitalWrite(m2a, LOW);
digitalWrite(m1b, LOW);
digitalWrite(m2b, LOW);
}