Benjamin Brou
dhfdf
Dependencies: mbed tsi_sensor TextLCD
main.cpp
- Committer:
- benbrou06
- Date:
- 2020-01-05
- Revision:
- 0:80e5a644fec8
File content as of revision 0:80e5a644fec8:
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
// Programme Robot Sans_Titre.jpg : //
///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////
#include <stdio.h>
#include "mbed.h"
#include <stdlib.h>
#include "TextLCD.h"
TextLCD lcd(D15, D14, D13, D12, D11, D10); // rs, e, d4-d7
//Décla globale des entrées et sorties
DigitalIn Jack(D3); //C'est le jack qu'il faudra retirer pour lancer le robot
PwmOut Motd(PTC8); //Moteur 1 = Moteur droit
PwmOut Motg(PTA13); //Moteur 2 = Moteur gauche
//Entrées analogiques des capteurs
AnalogIn Captd1(PTB1); //capteur droite 1
AnalogIn Captd2(PTB2); //capteur droite 2
AnalogIn Captg1(PTC2); //capteur gauche 1
AnalogIn Captg2(PTB3); //capteur gauche 2
AnalogIn CaptDist(PTE30); //capteur de distance
DigitalOut In1(D7); //Sens rotation moteur droit
DigitalOut In2(D9); //Sens rotation moteur gauche
DigitalIn FDC(D2); //Contact Fin de course (ou PTD4)
DigitalIn bp1(D5); //Boutons du IHM
DigitalIn bp2(D4);
DigitalOut ledR(LED_RED); //Déclaration Pwm des 3 leds de couleur Rouge, Verte et Bleu (dans l'ordre)
DigitalOut ledV(LED_GREEN);
DigitalOut ledB(LED_BLUE);
int main()
{
int b1,b2;
int state=0;
int mode=0;
float tcarre=0.6;
int Depart=0,etat=0;
float Vmg=0,Vmd=0; //Les 2 moteurs
float FinDeCourse=0;
int confettis;
float d1,d2,d3; //Delta entre les capteurs
float CG1,CG2,CD1,CD2,Cdist;
Motg.period_us(100);
Motd.period_us(100); //Decla des variables du main
Motg.pulsewidth_us(Vmg); //Vitesses des 2 moteurs
Motd.pulsewidth_us(Vmd);
printf("debut pgrm !\n\r"); //Test teraterm pour le début du prgm
/////////////////////////////////////////////////////////////////
/////Demarage du robot /////
/////Debut suivi de ligne /////
/////////////////////////////////////////////////////////////////
while (1) {
b1=bp1;
b2=bp2;
switch (state) { // gestion des états
case 0 :
if (b1==0) {
lcd.cls();
state=100;
}
break;
case 100 : //etat de transition de E0 vers E1
if (b1==1) {
state=1;
}
break;
case 1 : // choix 1 : bp1= suivant, bp2= confirmation
if (b1==0) {
state=12;
} else if (b2==0) {
state=13;
}
break;
case 11 :
if (b2==0 && b1==0) {
state=1000;
}
break;
case 12 :
if (b1==1) {
state=2;
}
break;
case 13 :
if (b2==1) {
state=11;
}
break;
case 2 : // choix 2 : bp1= suivant, bp2= confirmation
if (b1==0) {
state=22;
} else if (b2==0) {
state=23;
}
break;
case 21 :
if (b2==0 && b1==0) {
state=1000;
}
break;
case 22 :
if (b1==1) {
state=3;
}
break;
case 23 :
if (b2==1) {
state=21;
}
break;
case 3 : // choix 3 : bp1= suivant, bp2= reglage puis confirmation
if (b1==0) {
state=321;
} else if (b2==0) {
state=33;
}
break;
case 31 : //reglage taille carre et stockage dans la variable tcarre (float) | bp1= augmente taille carré, bp2= confirmation
if (b1==0) {
state=311;
} else if (b2==0) {
state=322;
} else if (tcarre==2.1) {
tcarre=0;
}
break;
case 311 :
if (b1==1) {
state=31;
tcarre+=0.1;
}
break;
case 32 :
if (b2==0 && b1==0) {
state=1000;
}
break;
case 321 :
if (b1==1) {
state=1;
}
break;
case 322 :
if (b2==1) {
state=32;
}
break;
case 33 :
if (b2==1) {
state=31;
}
break;
case 1000 : // etat de remise à 0
if (b2==1 && b1==1) {
state=0;
}
}
switch (state) { // gestion des sorties
case 0 : // case début choix
lcd.cls();
lcd.locate(0,0);
lcd.printf("Choisir");
lcd.locate(0,1);
lcd.printf(" mode");
ledR.write(1);
ledV.write(1); //Eteint!
ledB.write(1);
break;
case 1 :
lcd.cls();
lcd.locate(0,0);
lcd.printf("Suivi ?");
ledR.write(1);
ledV.write(1); //Eteint!
ledB.write(1);
break;
case 2 :
lcd.cls();
lcd.locate(0,0);
lcd.printf("Confetti");
lcd.locate(0,1);
lcd.printf(" ?");
break;
case 3 :
lcd.cls();
lcd.locate(0,0);
lcd.printf("Carre ?");
break;
case 11 : //suivi de ligne en cours
ledR.write(0);
ledV.write(1); //Rouge!
ledB.write(1);
lcd.cls();
lcd.locate(0,0);
lcd.printf("SDL en ");
lcd.locate(0,1);
lcd.printf("cours");
mode=1;
etat=0;
break;
case 21 : // mode confetti en cours
ledR.write(0);
ledV.write(0); //Blanc!
ledB.write(0);
lcd.cls();
lcd.locate(0,0);
lcd.printf("CFT en ");
lcd.locate(0,1);
lcd.printf("cours");
mode=2;
etat=0;
break;
case 31 : //reglage taille carre et stockage dans la variable tcarre (float)
lcd.cls();
lcd.locate(0,0);
lcd.printf("Taille : ");
lcd.locate(0,1);
lcd.printf("%gm",tcarre);
break;
case 32 : //mode carre en cours
ledR.write(1);
ledV.write(1); //Bleu!
ledB.write(0);
lcd.cls();
lcd.locate(0,0);
lcd.printf("CRE en ");
lcd.locate(0,1);
lcd.printf("cours");
mode=3;
etat=0;
break;
case 1000 : // etat de remise à 0
lcd.cls();
tcarre=0.6;
etat=0;
break;
}
if (mode==1) { //code du SDL
Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
CG1 = Captg1;
CG2 = Captg2;
CD1 = Captd1;
CD2 = Captd2;
Cdist = CaptDist;
d1 = CG2-CG1;
d2 = CG1-CD1;
d3 = CD1-CD2;
switch (etat) { // gestion des états
case 0 :
if (Depart==1) {
//printf("Depart \n\r");
etat=1;
}
break ;
case 1 : //roule tout droit
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if (CaptDist == 4) { //capteur Distance actif
etat=6;
} else if ((Captg1 < 0.1)&&(Captg2 < 0.1)) { //capteur CG1 et CG2 sont sur du blanc
etat=17;
} else if ((Captg1 < 0.1)&&(Captg2 < 0.1)&&(Captd1 < 0.1)&&(Captd2 < 0.1)) { //capteur CG1, CG2, CD1, CD2 sont sur du blanc
etat=9;
} else if (d2<-0.4) { //capteur CG1 sur du blanc
etat=2;
} else if (d2>0.4) { //capteur CD1 sur du blanc
etat=3;
}
break ;
case 17 : //état de transition (vers E7)
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if ((Captg1 > 0.1)&&(Captg2 > 0.1)) { //capteur CG1 et CG2 ne sont plus sur du blanc
etat=171;
}
break ;
case 171: //virage scotch coté gauche
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if ((Captg1 < 0.1)&&(Captg2 < 0.1)) {
etat=7;
}
break;
case 2 : //virage gauche
if (Depart == 0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if (d1<-0.4) {
etat=4;
} else if ((Captg1>0.1)&&(Captd1>0.1)) {
etat=1;
}
break ;
case 3 : //virage droit
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if (d3>0.4) {
etat=5;
} else if ((Captg1>0.1)&&(Captd1>0.1)) {
etat=1;
}
break ;
case 4: //virage ++ gauche
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if ((Captg1>0.1)&&(Captd1>0.1)&&(Captg2>0.1)&&(Captd2>0.1)) {
etat=1;
}
break;
case 5: //virage ++ droite
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if ((Captg1>0.1)&&(Captd1>0.1)&&(Captg2>0.1)&&(Captd2>0.1)) {
etat=1;
}
break;
case 6: //Capteur distance actif
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if (CaptDist != 4 ) {/*valeur à tester*/
etat=1;
}
break;
case 7: //Virage stotch
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
} else if ((Captg1>0.1)&&(Captg2>0.1)) {
etat=1;
}
break ;
case 9: //plot
if ((Captg1 < 0.1)&&(Captg2 < 0.1)&&(Captd1 < 0.1)&&(Captd2 < 0.1)) {
etat=1;
}
break ;
case 100 :
if (Depart==0) {
etat=0;
}
break ;
case 101 :
etat=0;
break ;
}
switch (etat) { // gestion des sorties
case 0 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
break ;
case 1 :
In1=0;
In2=1; //avance
Vmg=40;
Vmd=40;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 40%)
Motd.pulsewidth_us(Vmd);
break ;
case 17 :
Vmg=30;
Vmd=30;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 30%)
Motd.pulsewidth_us(Vmd);
break ;
case 2 :
Vmd=100*abs(d2)- 40; //Exemple si d2 = -0.8 (donc CG1 = 0.1 et CD1 = 0.9), alors la vitesse de la roue droite est 100*0.8-40 = 40
Vmg=Vmd-10;
Motd.pulsewidth_us(Vmd);
Motg.pulsewidth_us(Vmg);
break ;
case 3 :
Vmg=100*abs(d2)- 40; //Exemple si d2 = 0.8 (donc CG1 = 0.9 et CD1 = 0.1), alors la vitesse de la roue gauche est 100*0.8-40 = 40
Vmd=Vmg-10;
Motg.pulsewidth_us(Vmg);
Motd.pulsewidth_us(Vmd);
break ;
case 4 :
Vmd=100*abs(d1)- 50; //Exemple si d1 = -0.8 (donc CG2 = 0.1 et CG1 = 0.9), alors la vitesse de la roue droite est 100*0.8-50 = 30
Vmg=Vmd-10;
Motg.pulsewidth_us(Vmg);
Motd.pulsewidth_us(Vmd);
break;
case 5 :
Vmg=100*abs(d3)- 50; //Exemple si d3 = 0.8 (donc CD1 = 0.9 et CD2 = 0.1), alors la vitesse de la roue gauche est 100*0.8-50 = 30
Vmd=Vmg-10;
Motg.pulsewidth_us(Vmg);
Motd.pulsewidth_us(Vmd);
break;
case 6: //Capteur distance actif
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 0%)
Motd.pulsewidth_us(Vmd);
break;
case 7: //virage scotch coté gauche
Vmg=0;
Vmd=0;
wait(0.5);
Vmg=0;
Vmd=20;
Motg.pulsewidth_us(Vmg);
Motd.pulsewidth_us(Vmd);
break;
case 100 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
break ;
}
} //fin SDL
if (mode==2) { //code du mode confettis
Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
switch (etat) { // gestion des états
case 0 :
if (Depart==1) {
etat=1;
}
break ;
case 1 :
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
}
if ((Captg1<0.3)&&(Captd1<0.3)&&(Captg2<0.3)&&(Captd2<0.3)) {
confettis++;
wait(1);
}
if (confettis ==3) {
etat=0;
} else {
confettis=0;
}
break ;
case 100 :
if (Depart==0) {
etat=0;
}
break ;
case 101 :
etat=0;
break ;
}
switch (etat) { // gestion des sorties
case 0 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
ledR.write(0);
ledV.write(1); //Led = Rouge!
ledB.write(1);
break ;
case 1 :
Vmg=20;
Vmd=20;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 20%)
Motd.pulsewidth_us(Vmd);
ledR.write(1);
ledV.write(0); //Led = Verte!
ledB.write(1);
break ;
case 100 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
ledR.write(0);
ledV.write(1); //Led = Rouge!
ledB.write(1);
break ;
}
}//fin confettis
if (mode==3) { //code du mode carré
Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
switch (etat) { // gestion des états
case 0 :
if (Depart==1) {
etat=1;
}
break ;
case 1 :
if (Depart ==0) {
etat=101;
} else if (FinDeCourse==1) {
etat=100;
}
break ;
case 100 :
if (Depart==0) {
etat=0;
}
break ;
case 101 :
etat=0;
break ;
}
switch (etat) { // gestion des sorties
case 0 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
break ;
case 1 :
Vmg=20;
Vmd=20;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 20%)
Motd.pulsewidth_us(Vmd);
break ;
case 100 :
Vmg=0;
Vmd=0;
Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
Motd.pulsewidth_us(Vmd);
ledR.write(0);
ledV.write(1); //Led = Rouge!
ledB.write(1);
break ;
}
}//fin carré
}//fin de la boucle
}