|
/* 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);
|
|
}
|
|
}
|