Benjamin Brou
dhfdf
Dependencies: mbed tsi_sensor TextLCD
Diff: main.cpp
- Revision:
- 0:80e5a644fec8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jan 05 16:49:08 2020 +0000
@@ -0,0 +1,662 @@
+///////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////
+// 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
+}
\ No newline at end of file