Ugo Fighiera
C'est pour toi
Diff: main.cpp
- Revision:
- 0:d6e2c937698a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 20 18:56:23 2018 +0000
@@ -0,0 +1,730 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "Timer.h"
+#define moy 31 //rapport cyclique à baisser
+#define Kp 16
+#define Kd 1
+
+void vitG (int, int, float, float, float, float);
+void vitD (int, int, float, float, float, float);
+void Confettis();
+void Suivi_de_ligne(int,int);
+void Carre();
+
+DigitalIn fdc(D2);
+DigitalIn jack(D3);
+PwmOut moteur1(D6);
+PwmOut moteur2(D8);
+DigitalOut SensMot1(D7);
+DigitalOut SensMot2(D9);
+AnalogIn capteur1(A1);
+AnalogIn capteur2(A2);
+AnalogIn capteur3(A3);
+AnalogIn capteur4(A4);
+TextLCD lcd(D15,D14,D10,D11,D12,D13);
+DigitalIn Bouton1(D4);
+DigitalIn Bouton2(D5);
+
+int main()
+{
+ int bout_av1=0;
+ int bout_av2=0;
+ int bout_fdc=0;
+ int Prog=5, Sel=0, Regl=5, Vit=5, Coef=5;
+ int Moy=moy, kp=Kp;
+ while(1) {
+ switch(Sel) {
+ case 0 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Programm");
+ lcd.locate(0,1);
+ lcd.printf("e");
+ if(Bouton2.read() < bout_av2) {
+ Sel=1;
+ bout_av1=0;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Sel=1;
+ bout_av1=0;
+ wait(0.15);
+ break;
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ if(fdc.read() < bout_fdc) {
+ Sel=5;
+ Prog=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ break;
+ case 1 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Reglage");
+
+ if(Bouton2.read() < bout_av2) {
+ Sel=0;
+ bout_av1=0;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Sel=0;
+ bout_av1=0;
+ wait(0.15);
+ break;
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ if(fdc.read() < bout_fdc) {
+ Sel=5;
+ Regl=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ break;
+ default :
+ break;
+ }
+ switch(Prog) {
+ case 0 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Confetti");
+ lcd.locate(0,1);
+ lcd.printf("s");
+ if(fdc.read() < bout_fdc) {
+ Confettis();
+ Prog=5;
+ Sel=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Prog=3;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Prog=1;
+ wait(0.15);
+ break;
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ case 1 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Suivi de");
+ lcd.locate(0,1);
+ lcd.printf(" ligne");
+ if(fdc.read() < bout_fdc) {
+ Suivi_de_ligne(Moy,kp);
+ Prog=5;
+ Sel=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Prog=0;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Prog=2;
+ wait(0.15);
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ case 2 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Carre");
+ if(fdc.read() < bout_fdc) {
+ Carre();
+ Prog=5;
+ Sel=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Prog=1;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Prog=3;
+ wait(0.15);
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ case 3 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Quitter");
+ if(fdc.read() < bout_fdc) {
+ Prog=5;
+ Sel=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Prog=2;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Prog=0;
+ wait(0.15);
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ default :
+ break;
+ }
+ switch(Regl) {
+ case 0 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Vitesse");
+ if(fdc.read() < bout_fdc) {
+ Vit=0;
+ Regl=5;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Regl=2;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Regl=1;
+ wait(0.15);
+ break;
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ case 1 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Coeffici");
+ lcd.locate(0,1);
+ lcd.printf("ent");
+ if(fdc.read() < bout_fdc) {
+ Coef=0;
+ Regl=5;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Regl=0;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Regl=2;
+ wait(0.15);
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ case 2 :
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Quitter");
+ if(fdc.read() < bout_fdc) {
+ Regl=5;
+ Sel=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Regl=1;
+ wait(0.15);
+ break;
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Regl=0;
+ wait(0.15);
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ break;
+ }
+ switch(Vit) {
+ case 0 :
+ if(fdc.read() < bout_fdc) {
+ Vit=5;
+ Regl=0;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ Moy++;
+ if (Moy>=101) {
+ Moy=0;
+ }
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Vitesse:");
+ lcd.locate(0,1);
+ lcd.printf(" %d ",Moy);
+ wait(0.15);
+
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ Moy--;
+ if (Moy<=0) {
+ Moy=100;
+ }
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Vitesse:");
+ lcd.locate(0,1);
+ lcd.printf(" %d ",Moy);
+ wait(0.15);
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ default :
+ break;
+ }
+ switch(Coef) {
+ case 0 :
+ if(fdc.read() < bout_fdc) {
+ Coef=5;
+ Regl=1;
+ wait(0.15);
+ break;
+ }
+ bout_fdc=fdc.read();
+ wait_ms(10);
+ if(Bouton1.read() < bout_av1) {
+ kp++;
+ if (kp>50) {
+ kp=0;
+ }
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Coeffici");
+ lcd.locate(0,1);
+ lcd.printf("ent : %d",kp);
+
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ kp--;
+ if (kp<0) {
+ kp=50;
+ }
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Coeffici");
+ lcd.locate(0,1);
+ lcd.printf("ent : %d",Kp);
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ break;
+ default :
+ break;
+ }
+ }
+}
+
+void Confettis()
+{
+ moteur1.period_us(100);
+ moteur2.period_us(100);
+ int varJack ,etat=0 ;
+ float varCapt2, varCapt3;
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Confetti");
+ lcd.locate(0,1);
+ lcd.printf("s V");
+ while(etat!=3) {
+ varJack=jack.read();
+ varCapt2=capteur2.read();
+ varCapt3=capteur3.read();
+ switch(etat) {
+ case 0 :
+ if ( varJack == 1 ) { /* il n'y a plus le jack */
+ etat = 1;
+ }
+ break;
+ case 1 :
+ if ( varCapt2 <= 0.1 && varCapt3 <= 0.1 ) { /* Les capteurs détectent tout les deux LA zone "blanche" */
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Fini");
+ wait(0.15);
+ etat = 2;
+ }
+ break;
+ case 2 :
+ if ( varJack == 0 ) {
+ etat = 3;
+ }
+ break;
+ }
+ switch(etat) {
+ case 0 :
+ SensMot1=0;
+ SensMot2=0;
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ printf("Je suis a l'aret( de bus lol)\n\r");
+ break;
+ case 1 :
+ moteur1.pulsewidth_us(35);
+ moteur2.pulsewidth_us(35); // fonctionnement des moteurs
+ printf("Je roule\n\r");
+ break;
+ case 2 :
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ printf("J'attends\n\r");
+ break;
+ }
+ }
+}
+
+
+void Suivi_de_ligne(int Moy, int kp)
+{
+ int etat=0;
+ int varJack, varFDC;
+ float Erreur;
+ //int cpt=0;
+ lcd.cls();
+ moteur1.period_us(100);
+ moteur2.period_us(100);
+ // moteurs à l'arret
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ wait(0.001);
+ lcd.printf("Suivi de");
+ lcd.locate(0,1);
+ lcd.printf(" ligne V");
+ Timer t;
+ float dt,derive, Erreur_old;
+ t.start();
+ while(etat!=6) {
+ dt=t.read_us();
+ t.reset();
+ varJack=jack.read();
+ varFDC=fdc.read();
+ Erreur = capteur2.read()- capteur3.read();
+ derive=(Erreur-Erreur_old)/t;
+ switch(etat) {
+ case 0 : {
+ if ( varJack == 1 ) {
+ /* Il n'y a plus le jack */
+ etat = 1 ;
+ break;
+ }
+ break;
+ }
+ case 1 : {
+ if ( varFDC == 0 ) {
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Fini");
+ wait(0.15);
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ wait(0.15);
+ SensMot1=1;
+ wait(0.15);
+ moteur1.pulsewidth_us(25);
+ moteur2.pulsewidth_us(25);
+ wait(2);
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ SensMot1=0;
+ SensMot2=0;
+ etat = 2 ;
+ break;
+ }
+ if ( capteur1.read() <= 0.5 && capteur2.read() <= 0.5 && capteur3.read() <= 0.5 && capteur4.read() <0.5) {
+ break;
+ }
+ if ( capteur1.read() <= 0.5 && (capteur2.read() >= 0.5 || capteur3.read() >= 0.5)) {
+ etat = 3;
+ break;
+ }
+ if ( capteur4.read() <= 0.5 && (capteur2.read() >= 0.5 || capteur3.read() >= 0.5)) {
+ etat = 4;
+ break;
+ }
+ /*if ( capteur1.read() <= 0.5 && capteur2.read() <= 0.5 && capteur4.read() >= 0.5) {
+ cpt++;
+ if ( cpt == 2 ) {
+ etat = 5;
+ break;
+ }
+ wait(0.05);
+ break;
+ }*/
+ break;
+ }
+ case 2 : {
+ if ( varJack == 0 ) {
+ etat = 6 ;
+ break;
+ }
+ break;
+ }
+ case 3 : {
+ if ( capteur3.read()<=0.5 ) {
+ etat = 1;
+ break;
+ }
+ if ( varFDC == 0 ) {
+ etat = 2 ;
+ break;
+ }
+ break;
+ }
+ case 4 : {
+ if ( capteur2.read()<=0.5 ) {
+ etat = 1;
+ break;
+ }
+ if ( varFDC == 0 ) {
+ etat = 2 ;
+ break;
+ }
+ }
+ case 5 :
+ etat = 1;
+ break;
+
+ }
+
+ switch(etat) {
+ case 0 :
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ break;
+ case 1 :
+ vitG (Moy,kp,Erreur,dt,t,derive);
+ vitD (Moy,kp,Erreur,dt,t,derive);
+ break;
+ case 2 :
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ break;
+ case 3 :
+ moteur1.pulsewidth_us(10);
+ moteur2.pulsewidth_us(25);
+ break;
+ case 4 :
+ moteur1.pulsewidth_us(25);
+ moteur2.pulsewidth_us(10);
+ break;
+ /*case 5 :
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ wait(0.001);
+ SensMot1=1;
+ SensMot2=0;
+ moteur1.pulsewidth_us(25);
+ moteur2.pulsewidth_us(25);
+ wait(35*0.067);
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ SensMot1=0;
+ SensMot2=0;
+ break;*/
+ }
+ Erreur_old = Erreur;
+
+ }
+}
+
+
+void vitG (int Moy, int kp, float erreur, float d, float t, float d_erreur)
+{
+ int vitG;
+ vitG = Moy + (erreur*kp);
+ if ( vitG > 100 ) {
+ vitG = 100;
+ }
+ if ( vitG < 0 ) {
+ vitG = 0;
+ }
+ moteur1.pulsewidth_us((int)vitG);
+}
+
+void vitD (int Moy, int kp, float erreur, float d, float t, float d_erreur)
+{
+ int vitD;
+ vitD = Moy - (erreur*kp);
+ if ( vitD > 100 ) {
+ vitD = 100;
+ }
+ if ( vitD < 0 ) {
+ vitD = 0;
+ }
+ moteur2.pulsewidth_us((int)vitD);
+}
+
+
+
+
+void Carre()
+{
+ int bout_av1=0;
+
+ int bout_av2=0;
+ {
+ moteur1.period_us(100); /* moteur gauche */
+ moteur2.period_us(100); /* moteur droit */
+ int varJack, etat=0,iTaille, i;
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Taille :");
+ iTaille=60;
+ lcd.locate(0,1);
+ lcd.printf(" %d ",iTaille);
+ while(etat!=3) {
+ varJack=jack.read();
+ switch(etat) {
+ case 0 :
+ if(Bouton1.read() < bout_av1) {
+ iTaille++;
+ if (iTaille>200) {
+ iTaille=60;
+ }
+
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Taille :");
+ lcd.locate(0,1);
+ lcd.printf(" %d ",iTaille);
+
+ }
+ bout_av1=Bouton1.read();
+ wait_ms(10);
+ if(Bouton2.read() < bout_av2) {
+ iTaille--;
+ if (iTaille<60) {
+ iTaille=200;
+ }
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Taille :");
+ lcd.locate(0,1);
+ lcd.printf(" %d ",iTaille);
+ }
+ bout_av2=Bouton2.read();
+ wait_ms(10);
+ printf(" %d ",iTaille);
+ if ( varJack == 1) { /* Il n'y a plus le jack */
+ etat = 1;
+ break;
+ }
+ break;
+ case 1 :
+ if ( i == 4 ) {
+ lcd.cls();
+ wait(0.001);
+ lcd.printf("Fini");
+ wait(0.15);
+ etat = 2;
+ break;
+ }
+ case 2 :
+ if ( varJack == 0 ) {
+ etat = 3;
+ break;
+ }
+ }
+ switch(etat) {
+ case 0 :
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ break;
+ case 1 :
+ for (i=0; i<4; i++) {
+ SensMot1=0;
+ SensMot2=0;
+ moteur1.pulsewidth_us(26);
+ moteur2.pulsewidth_us(25); // fonctionnement des moteurs
+ wait(iTaille*0.018); // la taille du carré x le nombre de secondes par centimètre à 9V (75%)
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ wait(0.5);
+ SensMot1=0;
+ SensMot2=1;
+ moteur1.pulsewidth_us(23);
+ moteur2.pulsewidth_us(25);
+ wait((((37/2)*0.034)/2)+0.08); // besoin de la distance entre les deux roues
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0);
+ wait(0.5);
+ }
+ break;
+ case 2 :
+ SensMot1=0;
+ SensMot2=0;
+ moteur1.pulsewidth_us(0);
+ moteur2.pulsewidth_us(0); // moteurs à l'arrêt
+ break;
+ }
+ }
+ }
+}
\ No newline at end of file