Bonjour à tous,
J'ai un problème concernant mon robot qui devrai normalement faire ceci :
[On l'a auparavant déposé dans une pièce] :
- Toutes les 10 secondes, vérifier s'il y a quelqu'un en effectuant un ⅜ tour vers la droite, puis un ⅜ vers la droite (pendant ce temps, les leds/recepteurs infrarouges cherchent s'il y a quelqu'un)
- S'il y a quelqu'un :
{-le suivre jusqu'à ce qu'on le perde de vue(fonction que j'ai créé)
-quand on l'a perdu de vue, on s'arrête et on attends 10 secondes pour recommencer à chercher} - S'il n'y a personne :
{-attendre 10 secondes pour recommencer à chercher}
Ca a l'air tout simple mais il faut gérer beaucoup de choses en même temps ...
Mon problème est : quand j'exécute le programme, les servos font un petit bruit de vibration (comme si ils étaient à 1500 microseconds) et ne tournent pas, 10 secondes après, le robot tourne, détecte ma main, mais ne la suit pas vraiment comme il devrait le faire selon ma fonction follow()
A noter que toutes les fonctions que vous voyez (à part digitalRead() et tout) ont été créées par moi-même.
Voici mon code : (PS : fonctions.h est un fichier que j'ai créé qui contient plusieurs fonctions simple sur les servos)
#include <Servo.h>
#include "fonctions.h"
Servo servoLeft;
Servo servoRight;
int time;
int irLeft;
int irRight;
int a;
void setup()
{
servoLeft.attach(13);
servoRight.attach(12);
time = millis() + 9000;
}
void loop()
{
start_loop:
if (millis() >= time+10000) {
a = detect();
if (a == 0) {
time = millis();
goto start_loop;
}
if (a != 0) {
follow();
}
}
}
int irDetect(int irLedPin, int irReceiverPin, long frequency)
{
tone(irLedPin, frequency, 8);
delay(1);
int ir = digitalRead(irReceiverPin);
delay(1);
return ir;
}
int irDistance(int irLedPin, int irReceivePin)
{
int distance = 0;
for(long f = 38000; f <= 42000; f += 1000) {
distance += irDetect(irLedPin, irReceivePin, f);
}
return distance;
}
int irSignal() {
int irGauche;
int irDroite;
int c = 0;
while (irGauche == 5 && irDroite == 5 && c < 5000) {
irGauche = irDistance(9, 10);
irDroite = irDistance(2, 3);
delay (3);
c++;
}
if (c >= 5000) {
return 0;
}
if (irGauche != 5 || irDroite != 5) {
return 1;
}
}
int detect() {
int detectedLeft = 1;
int detectedRight = 1;
int count = 0;
int test = 1;
while(detectedLeft == 1 && detectedRight == 1 && count < 210) {
if (test == 1) {
turnRight(servoLeft, servoRight);
}
if (test == 2) {
turnLeft(servoLeft, servoRight);
}
if (count == 70){
test = 2;
}
delay(10);
detectedLeft = irDetect(9,10,38000);
detectedRight = irDetect(2,3,38000);
count++;
}
stopServo(servoLeft, servoRight);
if (detectedLeft == 0 || detectedRight == 0) {
return 1;
}
if (count >= 2100) {
return 0;
}
}
void follow() {
int fin = 0;
int left_delay = 0;
while (fin != 1) {
irLeft = irDistance(9,10);
irRight = irDistance(2,3);
if (irLeft > 2 && irLeft < 5 && irRight > 2 && irRight < 5) {
go(servoLeft, servoRight);
}
if (irLeft > 4 && irRight > 2 && irRight < 5) {
turnRight(servoLeft, servoRight);
delay(100);
}
if (irLeft > 2 && irLeft < 5 && irRight > 4) {
turnLeft(servoLeft, servoRight);
delay(100);
}
if (irLeft == 5 && irRight == 5) {
if (left_delay < 500) {
delay(100);
left_delay++;
}
if (left_delay >= 500) {
fin = 1;
}
}
if (irLeft <= 2 || irRight <= 3) {
goBack(servoLeft, servoRight);
delay(100);
}
}
stopServo(servoLeft, servoRight);
time = millis();
}
Contenu du fichier fonction.h :
void go (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1700);
servoRight.writeMicroseconds(1300);
}
void turnLeft (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1300);
}
void turnRight (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1700);
servoRight.writeMicroseconds(1700);
}
void goBack (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1700);
}
void pivotRight (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1700);
servoRight.writeMicroseconds(1500);
}
void pivotLeft (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1300);
}
void pivotBackRight (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1300);
servoRight.writeMicroseconds(1500);
}
void pivotBackLeft (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1700);
}
void stopServo (Servo servoLeft, Servo servoRight) {
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1500);
}
Merci d'avance pour vos réponses :3
