Problème pour asservir des servomoteurs

[t1412]

Bonjour,

Dans le cadre de mon TIPE, j'ai voulu modéliser, plutôt simplement, l'arrêt d'urgence des voitures autonomes. J'ai donc décidé d'asservir les servomoteurs du système de freinage avec un capteur à ultrason. Je précise que j'ai un niveau de débutant en programmation en C. J'ai obtenu le programme suivant :

#include <Servo.h>

// Définition des ports pour le capteur à ultrason

int trig = 13;
int echo = 12;

// Variables pour le capteur à ultrason

long duree;  // duree echo
float D;  //distance à l'obstacle
float V; //vitesse en cm/µs
float Vm; //vitesse en m/s
float Vk; //vitesse en km/h
float prevD; // distance précedente
unsigned long t; // date t


Servo myservo1;           // création de l'objet myservo
Servo myservo2;

//Définition port servomoteur 1

int pin_servo1 = 6;

//Définition port servomoteur 2

int pin_servo2 = 7;


void setup() {

  // initialisation position frein

  int position = 0;       // Position initiale
  myservo1.attach(pin_servo1);
  myservo1.write(position);
  myservo2.attach(pin_servo2);
  myservo2.write(position);

  // Initialisation du capteur à ultrason

  Serial.begin (9600);
  pinMode(trig, OUTPUT);
  digitalWrite(trig, LOW);
  pinMode(echo, INPUT);

  //Initialisation de la distance à l'obstacle

  digitalWrite(trig, LOW);
  delayMicroseconds(5);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duree = pulseIn(echo, HIGH); // Echo

  D = duree * 0.034 / 2; // calcule de la distance (le son parcourt environ 340 mètre par seconde soit 0.034 centimètre par microsecondes)

  Serial.print("Distance : ");
  Serial.print(D);
  Serial.println("cm");
}

//Fonction pour calculer la vitesse du véhicule

void vitesse () {

  t = millis(); // la nouvelle date t

  V = (prevD - D ) /  t ;  // calcul de la vitesse en cm/µs

  Serial.print(V, 3);
  Serial.print("cm/µs");
  Serial.print(" ");
  Serial.print(V * 1000000);
  Serial.print("cm/s :");
  Serial.println(" ");

  Vm = V * 10000; // calcul de la vitesse en m/s

  Serial.print(Vm, 3);
  Serial.print("m/s :");
  Serial.print(" ");

  Vk = Vm * 3.6;    // calcul de la vitesse en km/h

  Serial.print(Vk, 3);
  Serial.print("km/h");
  Serial.print(" ");
}

void loop() {

  //Calcul de la vitesse

  prevD = D; // on donne la valeur précedente de D

  digitalWrite(trig, LOW);
  delayMicroseconds(5);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duree = pulseIn(echo, HIGH); // Echo

  Serial.print("durée: ");
  Serial.print(duree);
  Serial.print("µs;");

  D = duree * 0.034 / 2; // calcule de la distance (le son parcourt environ 340 mètre par seconde soit 0.034 centimètre par microsecondes)

  // Affichage
  Serial.print("Distance : ");
  Serial.print(D);
  Serial.print("cm");
  Serial.println(" ");
  vitesse();


  // Freinage

  if ( (2 < D < 20) and (Vk > 0.2)) {

    int position = 180;       // Position des freins pour le freinage

    myservo1.attach(pin_servo1);
    myservo1.write(position);
    myservo2.attach(pin_servo2);
    myservo2.write(position);

  }

  else {
    int position = 0;
    myservo1.attach(pin_servo1);
    myservo1.write(position);
    myservo2.attach(pin_servo2);
    myservo2.write(position);

    Serial.print(" ");
    Serial.print(position);
    Serial.println(" ");

    delay (300);   //Pour éviter que l'affichage de l'ensemble des valeurs ne freeze
  }
}

Cependant, les servomoteurs ne réagissent pas à la condition du "if". Je ne vois pas comment régler ce problème. Merci pour votre aide.

[JiPe38]

Vous faites un gros bug lorsque vous écrivez :

t = millis(); // la nouvelle date t
V = (prevD - D ) / t ; // calcul de la vitesse en cm/µs

IL faut diviser par l'intervalle de temps et non le temps absolu (t).

Votre test if (2 < D < 20) devrait être remplacé par
if (2 < D && D < 20)

Avec votre calcul de freinage, la voiture va soit s'arrêter bien avant l'obstacle, soit après. Je vous avais donné dans un post précédent une stratégie pour un freinage tout ou rien optimisé, mais je suppose que vous n'y avez rien compris. Vous n'avez pas vu les formules de mouvement uniformément accéléré en terminale ?