//programme de detection de danger par capteur ultrat sonord
//résiste au décablage d'un capteur , le m'et hors service sans pertuber les autres capteur
//résiste au valeur ponctuelle avec un buffer de donnée ( ne decte pas une mouche qui passe...)
// variable temporelle
float tempjeu = 0.0;//temps depuis le lenssement du jeux
float tempprog = 0.0;// temps soustrayant les poses du aux arrets imprévus sur le parcour et les periodes de rotation.
const float finjeu = 100000.0; // temps de fin de jeux en mili seconde arréte le robot dans tout ces déplacement et action
float tempboucle = 0.0; // temp entre chaque boucle pour calculer l'étape la position à chaque boucle
float ecartp = 0.0; // variable calcule servant de repaire au tempboucle
//variable de condition
int contact = 1; // varible on off coup d'envois
int danger = 0;// détection d'un obstacle imprévus arrêt du véicule
//variable de calcule spatial
const float sud = 1500*PI;const float nord = PI*500;const float est = 0.0;const float ouest = PI*1000;// orientation primaire utile en milli radiant
const float vmaxrec = 0.05;// vitesse max sur sol plat en mm par milli seconde
const float vmaxcirc = PI/12;//vitesse max circulaire sur sol plat en milliradant par milliseconde
float x = 0.0; // position d'origine en x en mm
float y = 0.0; // position d'origine en y en mm
float oriente ;//angle de destination à l'étape doné
float n = 2.0;// marge de degrée pr arivé à l'orientation voulu se calcule en faisant vmaxcirc*tempbouclemax
float tempbouclemax = 0; //permet de calculé la magre de degrée n en fonction de la vitesse minimal du programe
float degresReel = PI*500; //direction de la tête du robot dans le repére x y en mradiant radiant au démarage
//Câblage moteur 1
int ENAun=8; int IN1un=9; int IN2un=10; int ENBun=13; int IN3un=11; int IN4un=12;
//cablaage moteur 2
int ENAdeux=2; int IN1deux=3; int IN2deux=4; int ENBdeux=7; int IN3deux=5; int IN4deux=6;
// etape de 1 à 8 des moteurs pas à pas
int etapemoteurun = 1;int etapemoteurdeux =1;
//asignation des variables des capteurs ultrat sonord
const byte TRIGGERun = 52;const byte ECHOun = 53; //broche de transmission par numéro de capteur.
const byte TRIGGERdeux = 50; const byte ECHOdeux = 51;
const byte TRIGGERtrois = 48; const byte ECHOtrois = 49;
const byte TRIGGERquatre = 46; const byte ECHOquatre = 47;
const byte TRIGGERcinq = 44; const byte ECHOcinq = 45;
const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s constante de calcule
const float SOUND_SPEED = 340.0 / 1000; //vitesse du son
long measureun;long measuredeux;long measuretrois;long measurequatre;long measurecinq; //doné récupéré
float distanceun;float distancedeux;float distancetrois;float distancequatre;float distancecinq; //distance déduite
int bufer = 5; //le risque doit étre vérifier cinq fois pr étre valide
int margesecu =200; //les distance de sécurité entre le robot et les obstacle
//tableau des trajectoire
int etape = 0; // étape dans l'avancement du programme
const int B = 5; //nombre d'étape
float parcour [3][B]{{500.0,600.0,600.0,200.0,4000.0},// déterminé la distance en mm
{nord,est,est,ouest,PI*250},//déterminer la direction tableau servant d'interface pour stocker la trajectoire et les action à réaliser en degrée
{0.0,0.0,0.0,0.0,0.0,}}; // temprograme équivalent à l'éatape
//led de test sans moniteur
int pinledtest =36;
void setup() {
Serial.begin(2000000);
//configurer led
pinMode(pinledtest,OUTPUT);
//configurer les sortit moteur
pinMode(ENAun,OUTPUT);pinMode(ENBun,OUTPUT);pinMode(IN1un,OUTPUT);pinMode(IN2un,OUTPUT);pinMode(IN3un,OUTPUT);pinMode(IN4un,OUTPUT);//Les 6 pins configurées en sorties
pinMode(ENAdeux,OUTPUT); pinMode(ENBdeux,OUTPUT);pinMode(IN1deux,OUTPUT);pinMode(IN2deux,OUTPUT);pinMode(IN3deux,OUTPUT);pinMode(IN4deux,OUTPUT);//Les 6 pins configurées en sorties
//Enable A et B, utiliser les 2 ponts en H
digitalWrite(ENAun,HIGH);digitalWrite(ENBun,HIGH);digitalWrite(ENAdeux,HIGH);digitalWrite(ENBdeux,HIGH);
// Initialise les broches des capteur ultrat sonord
pinMode(TRIGGERun, OUTPUT);digitalWrite(TRIGGERun, LOW); pinMode(ECHOun, INPUT);
pinMode(TRIGGERdeux, OUTPUT);digitalWrite(TRIGGERdeux, LOW);pinMode(ECHOdeux, INPUT);
pinMode(TRIGGERtrois, OUTPUT);digitalWrite(TRIGGERtrois, LOW); pinMode(ECHOtrois, INPUT);
pinMode(TRIGGERquatre, OUTPUT);digitalWrite(TRIGGERquatre, LOW); pinMode(ECHOquatre, INPUT);
pinMode(TRIGGERcinq, OUTPUT);digitalWrite(TRIGGERcinq, LOW); pinMode(ECHOcinq, INPUT);
//remplire la derniére ligne du tableau
oriente = parcour [1][0];
parcour [2][0]= parcour [0][0] / vmaxrec;
for (int i = 0 ; i <= B-1 ; i++){
parcour [2][i+1] = parcour [2][i] + (parcour [0][i+1] / vmaxrec);}
}
void loop() {
// detecter le coup denvois
if (contact == 1){
//prise des valeurs de distance par les capteurs ultrat sonord//
digitalWrite(TRIGGERun, HIGH);digitalWrite(TRIGGERun, LOW);long measureun= pulseIn(ECHOun, HIGH, MEASURE_TIMEOUT);float distanceun = measureun/ 2.0 * SOUND_SPEED;
digitalWrite(TRIGGERdeux, HIGH);digitalWrite(TRIGGERdeux, LOW);long measuredeux= pulseIn(ECHOdeux, HIGH, MEASURE_TIMEOUT);float distancedeux = measuredeux/ 2.0 * SOUND_SPEED;
digitalWrite(TRIGGERtrois, HIGH);digitalWrite(TRIGGERtrois, LOW);long measuretrois= pulseIn(ECHOtrois, HIGH, MEASURE_TIMEOUT);float distancetrois = measuretrois/ 2.0 * SOUND_SPEED;
digitalWrite(TRIGGERquatre, HIGH);digitalWrite(TRIGGERquatre, LOW);long measurequatre= pulseIn(ECHOquatre, HIGH, MEASURE_TIMEOUT);float distancequatre = measurequatre/ 2.0 * SOUND_SPEED;
digitalWrite(TRIGGERcinq, HIGH);digitalWrite(TRIGGERcinq, LOW);long measurecinq= pulseIn(ECHOcinq, HIGH, MEASURE_TIMEOUT);float distancecinq = measurecinq/ 2.0 * SOUND_SPEED;
// detecter un obstacle imprévut celon margesecu et avec un bufer ignorant les detection ponctuel
if((((distanceun>0)&&(distanceun<margesecu))|| ((distancedeux>0)&&(distancedeux<margesecu))||((distancetrois>0)&&(distancetrois<margesecu))||((distancequatre>0)&&(distancequatre<margesecu))||((distancecinq>0)&&(distancecinq<margesecu)))&&(bufer<=10)){
bufer += 1; }
else{if (bufer>0){bufer -=1;}}
if (bufer>5){danger=1;}
else{danger =0;}
//lecture détape //
if ( tempprog >= parcour [2][etape]){etape = etape + 1;}// faire evoluer la trajectoire
oriente = parcour [1][etape];//lire l'orientation visé
if (danger == 0){
//gestion spatial//
//determiné la marge de précision de rotation en fonction de la vitesse du programe
if ((tempboucle>tempbouclemax)&&(tempprog>1)){tempbouclemax = tempboucle;
n = tempbouclemax*vmaxcirc/2;}
//gestion d'avancé rectiligne
if(((cos(oriente/1000)<cos(n/1000))&&(abs(oriente-degresReel)<=n))||((oriente-n<=0)&&((degresReel<=oriente+n)||(degresReel>=oriente-n+2000*PI)))||((oriente+n>=2000*PI)&&((degresReel<=oriente+n-2000*PI)||(degresReel>=oriente-n)))){ //si degresReel convient à l'orientation demandé avec une marge n (prenant en conte les écart n entre N et 2PI- n)
/* x = x + cos(degresReel / 1000) * tempboucle * vmaxrec ;//calculé x y théorique en m m, possiblement remplacé par un accélérométre dans la boucle principal
y = y + sin(degresReel / 1000) * tempboucle * vmaxrec ;*/
if(millis()%20<=tempbouclemax){
etapemoteurun += 1;
etapemoteurdeux -= 1;
aplicationmoteur();}
}
la suite arive