Mon contenu
Homme
Posté par Bonjour,
J'ai écrit un petit programme tout bete histoire de voir comment on pouvait utilliser ROS avec Arduino a l'aide de rosserial, et du coup j'aimerais bien avoir votre avis sur ce que j'ai fait et si c'est la bonne facon de faire s'il vous plait.
Ce que le programme fait :
Programme écrit sur l'IDE Arduino :
- Je récupère la durée d'impulsion d'un capteur ultrason HC-SR04.
- Je publie cette donnée dans un topic nommé "sensorDistance"
Programme écrit sur mon IDE C++ (Clion):
- Je sub au topic "sensorDistance" afin de récuperer la durée d'impulsion publié par le MC Arduino.
- J'effectue un calcul pour connaitre la distance de l'obstacle.
- Je publie cette distance dans un topic "motorSpeed"
- En fonction de la distance je décide soit d'avancer soit de tourner a droite.
- Ayant un tableau PWM[2] (car il y'a deux moteurs), si le robot avance je met les valeur a PWM[0] = 150 ,PWM[1] = 150, sinon je met PWM[0] = 150 , PWM[1] = 0
- Je publie le tableau PWM dans un topic nommé "motorControl"
Programme écrit sur l'IDE Arduino :
- Je sub au topic " motorControl" afin de récuperer le tableau PWM.
- Et j'appelle la fonction "avancer" ou "tournerAdroite" en attribuant le bon PWM au moteurs.
Je vais juste mettre le code écrit sur l'IDE Arduino car c'est la ou je suis le moins expérimenté:
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
// Motor A
int enA = 3;
int in1 = 2;
int in2 = 4;
// Motor B
int enB = 5;
int in3 = 6;
int in4 = 7;
// defines pins numbers
const int trigPin = 11;
const int echoPin = 12;
ros::NodeHandle myNode;
std_msgs::Float64 pulseDuration;
void forward(const std_msgs::Float64MultiArray& PWM){
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
analogWrite(enA,PWM.data[0]);
analogWrite(enB,PWM.data[1]);
}
void backWard(const std_msgs::Float64MultiArray& PWM){
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
analogWrite(enA,PWM.data[0]);
analogWrite(enB,PWM.data[1]);
}
void turnRight(const std_msgs::Float64MultiArray& PWM){
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
analogWrite(enA,PWM.data[0]);
analogWrite(enB,PWM.data[1]);
}
void motorCallBack(const std_msgs::Float64MultiArray& PWM){
Serial.print("PWM1=");
Serial.println(PWM.data[0]);
Serial.print("PWM2=");
Serial.println(PWM.data[1]);
if(PWM.data[0] > 0 && PWM.data[1] > 0){
forward(PWM);
}else{
turnRight(PWM);
}
}
ros::Subscriber<std_msgs::Float64MultiArray> subscriber("motorControl",&motorCallBack);
ros::Publisher publisher("sensorDistance",&pulseDuration);
void setup()
{
pinMode(enA,OUTPUT);
pinMode(enB,OUTPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
myNode.initNode();
myNode.subscribe(subscriber);
myNode.advertise(publisher);
Serial.begin(57600);
}
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
pulseDuration.data = pulseIn(echoPin, HIGH);
publisher.publish(&pulseDuration);
myNode.spinOnce();
delay(1000);
}
Merci.
,
