Project

General

Profile

Files » Gestionnaire_PR_ULRIME.ino

Justin Cano, 05/14/2016 09:30 AM

 
/* Programme gestionnaire du Petit Robot
Armand Sibille, le 22-12-2015
Tarek BENHNINI & Justin CANO, le 30-03-2016


I - Constantes globales */
int declenchement = A3;
int parasol = 11;
int belier = A7;
int capteurG = A0;
int capteurD = A2;
int capteurA = A1;
int ledA = 2;
int ledB = 3;
int ledC = 4;

boolean testRouge;
float vitesseTranslation=0.0;
float vitesseRotation=0.0;
float variationMotorG=0.0;//distance parcourue par l'action du moteur droit
float variationMotorD=0.0;//distance parcourue par l'action du moteur gauche
boolean actualise=false;//permet de savoir si l'arduino d'asservissement a actualisé variationMotorG et variationMotorD

long tempsDebut = 0;

//communication i2c
boolean req=false;//permet de savoir où on en est de l'envoie des consignes
#include <Wire.h>

//commande de servomoteurs
#include<Servo.h>
Servo myservo; //servomoteur du parasol (objet crée)
int capteurL;


// II - Méthodes


void setup () {
vitesseTranslation = 0.0;
vitesseRotation=0.0;
pinMode(parasol,OUTPUT);
pinMode(ledA,OUTPUT);
pinMode(ledB,OUTPUT);
pinMode(ledC,OUTPUT);
myservo.attach(parasol);
myservo.write(90); //intialisation du parasol;
delay(500);
myservo.detach();
Serial.begin(9600); //Initialisation de la liaison Sérielle
}
boolean demarrage = false;

void testo () {
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
}


void loop () {
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
boolean rouge = servietteRouge();
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
demarrage = cable();
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
//boolean demarrage = false;
if(demarrage) {
Wire.begin(8); // join i2c bus with address #8
Wire.onReceive(receiveEvent); // register event
Wire.onRequest(requestEvent); // request event
digitalWrite(ledA,HIGH);
run(rouge);
}
if(rouge) {
digitalWrite(ledB,HIGH);
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
digitalWrite(ledC,HIGH);
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
}
else {
digitalWrite(ledC,LOW);
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
digitalWrite(ledB,HIGH);
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
}
vitesseRotation = 0.0;
vitesseTranslation = 0.0;

//delay(1000);
//raseMur(true);
//pivot(false);
//arret(true);
}

void run(boolean rouge) {
tempsDebut = millis();
raseMur(rouge);
pivot(rouge);
raseMur2(rouge);
raseMur3(rouge);
tourneFerme(rouge);
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
arretParasol();
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
ouvreParasol();
vitesseRotation =0.0;
vitesseTranslation = 0.0;
while(true) {
delay(1);
}
}

// Methodes JC Necessaires
boolean checkTime() {
long tempsAbsolu = millis() - tempsDebut;
boolean b = boolean(tempsAbsolu < 90000);
return b;
}



boolean servietteRouge() {
float G = 0;
float D = 0;
for( int i = 0; i<100; i++) {
G+= distance(capteurG);
D+= distance(capteurD);
delay(2);
}
boolean b = (G<D);
return b;
}

void decolage(boolean rouge) {
int T1 = 1000;
float VT = 10.0;
float VR = 5.0;
long tempsEtape = millis();
while(millis() - tempsEtape < T1 && checkTime()) {
if(rouge) {
VR = -VR;
}
vitesseTranslation = VT;
vitesseRotation = VR;
delay(1);
}
}

void raseMur(boolean rouge) {
float dMin = 3.7;
float dMax = 8.0;
//float dArret = 3.0;
float dArret = 4.0;
float correction = 0.4;
int T = 100;

float vitesseTranslation_ = 5.0;
if(rouge) {
capteurL = capteurG;
}
else {
capteurL = capteurD;
correction = -0.3*correction;
dArret = 3.8;
dMin = 3.2;
dMax = 7.4;
}
while(distance(capteurA)>dArret && checkTime() ) {
vitesseTranslation = vitesseTranslation_;
if (dMin > distance(capteurL)) {
//evitLR();
vitesseTranslation = 0.0;
vitesseRotation=-correction;
}
else if (dMax < distance(capteurL) ) {
//evitLR();
vitesseTranslation = 0.0;
vitesseRotation = correction;
}
delay(T);
vitesseRotation=0.0;
}
}

void pivot(boolean rouge) {

int tempsRotation = 1300;
float VR = 1.0;
long tempsEtape = millis();
int capteurL;
int capteurE;
float delta;
if(rouge) {
VR = -VR;
capteurL = capteurG;
capteurE = capteurD;
}
else{
tempsRotation=1300;
capteurL = capteurD;
capteurE = capteurG;
}
vitesseTranslation = 0.0;
float seuilImmobilite = 0.4;
boolean b = true;

while(millis()-tempsEtape < tempsRotation && checkTime()) {
while(distance(capteurE) < 3.0) {
vitesseRotation = 0;
}
//evit();
vitesseRotation = VR;
if(b) {
long tempsBoucle = millis();
delay(400);
float mesure1 = distance(capteurA);
delay(400);
float mesure2 = distance(capteurA);
delta = abs(mesure1 - mesure2);
}
if(delta< seuilImmobilite && b) {
vitesseTranslation = -5.0;
delay(800);
//evit();
vitesseTranslation = 0.0;
tempsEtape = millis();
b = false;
}

}
vitesseRotation = 0.0;
}

void raseMur2(boolean rouge) {
float dMin = 3.0;
float dMax = 4.5;
float dArret = 3.0;
float correction = 1.0;
int T = 4000;
int T2 = 3600;
float vitesseTranslation_ = 7.0;
if(rouge) {
capteurL = capteurG;
}
else {
capteurL = capteurD;
correction = -correction;
dMin = 3.2;
dMax = 4.5;
T = 2400;
T2 = 2000;
}
long temps = millis();
while(distance(capteurA)>dArret && millis() - temps < T && distance(capteurL) < 16.0 && checkTime() ) {
//evit();
vitesseTranslation = vitesseTranslation_;
vitesseRotation=0.0;
}
temps = millis();
while( millis() - temps < T2 && checkTime() ) {
//evit();
vitesseTranslation = 0.0;
vitesseRotation = correction;
}
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
}


void raseMur3(boolean rouge) {
float dMin = 3.0;
float dMax = 8.0;
float dArret = 3.0;
float correction = 0.0;
int T = 5000;

float vitesseTranslation_ = 5.0;
if(rouge) {
capteurL = capteurG;
}
else {
capteurL = capteurD;
correction = -correction;
dMin = 2.0;
dMax = 5.0;
}
long time = millis();
while(distance(capteurA)>dArret && checkTime() && millis() - time < T ) {
//evit();
vitesseTranslation = vitesseTranslation_;
vitesseRotation = 0.0;
}
}

void tourneFerme(boolean rouge) {
float rotation = 0.5;
int temps = 20000;
vitesseTranslation = 0.0;
long tempsEtape = millis();
long t = 0;
if(rouge) {
rotation = -rotation;
}
while( t < temps && checkTime()) {
t = millis() - tempsEtape;
if(t > 0.33*temps) {
vitesseRotation = -rotation;
}
else{
vitesseRotation = rotation;
}
}



vitesseTranslation = 0.0;
vitesseRotation = 0.0;
}

void arret(boolean b) {
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
while(true) {

vitesseRotation = 0.0;
vitesseTranslation = 0.0;
}
}

void arretParasol() {
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
while(checkTime()){
delay(100);
}
//Wire.endTransmission(8);
}




float distance(int pin) {
int a = analogRead(pin);
float b = distanceIr(a);
return b;
}
float distanceIr(int tensionCodee) {
//demander le polynôme à Zengpeng
// Distance en mm donnée par l'IR avec pour entrée un analogRead
/* Implémentation du polynôme de Z.Zhou par J.Cano 3/3/16 : Il existe deux modèles selon deux régions distinctes et un intervalle de validité des mesures
lisez la documentation dans le google drive, que nous mettrons bien entendu dans le Wiki de la forge sous peu.*/
// Convention : on évoque les tensions en points de mesures de 0 à 1024
int delimitation_region = 52;
float c1 = 811.0;
float d1 = 27.3;
float e1 = 4.29;
float b1 = -84.5;
float a1 = 2040.0;
float distance = 1.0;
tensionCodee=constrain(tensionCodee,28,700); //intervalle pour "casser" les mesures abérantes
if(tensionCodee<delimitation_region) {
distance = -e1*log((tensionCodee-d1)/c1);
}
else {
distance = a1/(tensionCodee-b1);
}
distance = constrain(distance,0,30); // pour éviter que la distance diverge trop
return distance;
}

boolean cable() {
int pin_r = declenchement;
boolean b = !digitalRead(pin_r);
int j = 0;
while(b && j < 10) {
boolean b = !digitalRead(pin_r);
delay(100);
j++;
}
return b;
}

boolean recepteur () {
//On tente de recevoir 110101 à 500 Hz, tant qu'on l'a pas reçu on attend !
boolean r = false;
int pin_r = declenchement; // adaptation du nom des variables
boolean b, c, d, e, f;
while(!r) {
boolean a = digitalRead(pin_r);
delay(1);
if(a) {
a = digitalRead(pin_r);
delay(2);
b = digitalRead(pin_r);
delay(2);
c = digitalRead(pin_r);
delay(2);
d = digitalRead(pin_r);
delay(2);
e = digitalRead(pin_r);
delay(2);
f = digitalRead(pin_r);
Serial.print(a);
Serial.print(b);
Serial.print(c);
Serial.print(d);
Serial.print(e);
Serial.println(f);
delay(500);
//Test : a-t-on reçu 110101 à 500 Hz
r = a && b && !c && d && !e && f;
}
}
return r;
}

void ouvreParasol() {//ouverture du parasol
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
myservo.attach(parasol); // On bloque le servo sur la pin parasol
myservo.write(
1); //On fait bouger le servo (libération du parasol) à environ 0 degrès (position décenchement)
delay(1500); // On attend un temps suffisament long pour que le ressort & le servo fassent leur effet
myservo.write(180); // On le remet en position initiale (180) , prêt à recharger le ressort du parasol
delay(1500); // On attend que le servo fassent leur effet
myservo.detach(); // on débloque le servo
while(true){
// Petite annimation de fin de leds pour épater la gallerie
for(int i = 7; i>1; i--){
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
int s = i*100;
digitalWrite(ledA,HIGH);
delay(s);
digitalWrite(ledB,HIGH);
delay(s);
digitalWrite(ledC,HIGH);
delay(s);
digitalWrite(ledC,LOW);
delay(s);
digitalWrite(ledB,LOW);
delay(s);
digitalWrite(ledA,LOW);
}
// boucle dans lequel le programme va "tomber" à la fin -> Arrêt total du robot à T = 93s + quelques centièmes
}
}


void receiveEvent(int howMany) {
//se déclenche lorsque l'arduino d'asservissement envoie les valeurs du retour moteur
int c = Wire.read(); // receive byte as an int
variationMotorG=((float) c)/10.0;
c = Wire.read(); // receive byte as an int
variationMotorD=((float) c)/10.0;
actualise=true;
}

void requestEvent() {
//se déclenche lorsque l'arduino d'asservissement demande les consignes
if (req==false) {//envoi consigne translation
constrain(vitesseTranslation,-12.5,12.5);
int translation=(int)(vitesseTranslation*10.0+125);
Wire.write(translation);
//Serial.print(i);
//Serial.print(" ");
req=true;
} else if (req==true) {//envoi consigne rotation
constrain(vitesseRotation,-1.0,1.0);
int rotation= (int)(vitesseRotation*100.0 +100.0);
req=false;
Wire.write(rotation);
//Serial.println(x);
}
}

void evit() {

float mesureA = distance(capteurA);
float seuil = 2.7;
boolean b = mesureA < seuil;

if(b) {
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
delay(500);
}
}

void evitLR() {
float mesureG = distance(capteurG);
float mesureD = distance(capteurD);
//float mesureA = distance(capteurA);
float seuil = 3.0;
boolean b = mesureG < seuil ||
mesureD < seuil;
if(b){
vitesseRotation = 0.0;
vitesseTranslation = 0.0;
delay(500);
}
}
(7-7/8)