dorian diana / Mbed 2 deprecated robot_N1N0

ER2

Dependencies:   mbed SRF05

main.cpp@2:354d20277612, 2021-06-02 (annotated)

Committer:
dorian06
Date:
Wed Jun 02 07:21:07 2021 +0000
Revision:
2:354d20277612
Parent:
1:06c976cc11fb 
N1N0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 0:94fe153be5b7 1 #include "mbed.h"
dorian06 2:354d20277612 2 #include "SRF05.h"
dorian06 2:354d20277612 3 #define PI 3.14159265358979323846264338327950288419716939937510582
dorian06 2:354d20277612 4 #define periode 20
dorian06 2:354d20277612 5 #define Temps 1.0 //temps sur le quel on mesure le nombre d'encodeur passé
dorian06 2:354d20277612 6 #define rayon 0.021
dorian06 2:354d20277612 7 #define crans 12
dorian06 2:354d20277612 8 #define voulu 25.0
dorian06 2:354d20277612 9 SRF05 srf(p18, p17);
dorian06 2:354d20277612 10 //Déclaration des variables globales
dorian06 2:354d20277612 11 DigitalOut myled1(LED1) ;
dorian06 2:354d20277612 12 DigitalOut myled2(LED2) ;
dorian06 2:354d20277612 13 DigitalOut myled3(LED3) ;
dorian06 2:354d20277612 14 DigitalOut myled4(LED4) ;
dorian06 2:354d20277612 15 DigitalIn Bp1(p30); //gauche
dorian06 2:354d20277612 16 DigitalIn Bp2(p29); //droit
dorian06 2:354d20277612 17 AnalogIn capt1(p20); //droit
dorian06 2:354d20277612 18 AnalogIn capt2(p19); //gauche
dorian06 2:354d20277612 19 InterruptIn Encodeur_AG(p10);
dorian06 2:354d20277612 20 InterruptIn Encodeur_AD(p9);
dorian06 2:354d20277612 21 InterruptIn Encodeur_BG(p7);
dorian06 2:354d20277612 22 InterruptIn Encodeur_BD(p6);
dorian06 2:354d20277612 23 PwmOut mot1(p23);
dorian06 2:354d20277612 24 PwmOut mot2(p22);
dorian06 2:354d20277612 25 DigitalOut sens_droit(p11);
dorian06 2:354d20277612 26 DigitalOut sens_gauche(p12);
dorian06 2:354d20277612 27 AnalogIn Vbat(p15);
dorian06 2:354d20277612 28 float dist;
dorian06 2:354d20277612 29 int sens=1;
dorian06 2:354d20277612 30 int cpt=0;
dorian06 2:354d20277612 31 int vitesse1=30;
dorian06 2:354d20277612 32 int vitesse2;
dorian06 2:354d20277612 33 int mot;
dorian06 2:354d20277612 34 int ticker;
dorian06 2:354d20277612 35 float vitessemsAG; //vitesse en m/s en fonction des encodeurs
dorian06 2:354d20277612 36 float vitessemsAD;
dorian06 2:354d20277612 37 float vitesseradsAG; // vitesse en ras/s en foction des encodeurs
dorian06 2:354d20277612 38 float vitesseradsAD;
dorian06 2:354d20277612 39 int cpt1=0,cpt2=0; // compteur pour la mesure de la vitessse
dorian06 2:354d20277612 40 Ticker T1; // ticker pour la mesure de la vitesse
dorian06 2:354d20277612 41 float Bat;
dorian06 2:354d20277612 42 void sens_vitesse(int,int); // gestin du sens et de la vitesse et du moteur 1 periode entre [-100;100] puis le moteur 1=droit 2=gauche
dorian06 2:354d20277612 43 void gestion_cpt_AG(void); //mesure de la vitesse sue les different encodeurs
dorian06 2:354d20277612 44 void gestion_cpt_AD(void); //" "
dorian06 2:354d20277612 45 void calculvitesse(void);
dorian06 2:354d20277612 46 void position(float); // distance voulu
dorian06 2:354d20277612 47 int main() //Début du main
dorian06 2:354d20277612 48 {
dorian06 2:354d20277612 49 Encodeur_AG.mode(PullNone); // pas de resistance interne
dorian06 2:354d20277612 50 Encodeur_AD.mode(PullNone);
dorian06 2:354d20277612 51 Encodeur_AG.rise(&gestion_cpt_AG); // front montant
dorian06 2:354d20277612 52 Encodeur_AD.rise(&gestion_cpt_AD);
dorian06 2:354d20277612 53 Bp1.mode(PullUp); //Déclaration des variables locales au main
dorian06 2:354d20277612 54 Bp2.mode(PullUp);
dorian06 2:354d20277612 55 mot1.period_us(100); //periode des moteurs
dorian06 2:354d20277612 56 mot2.period_us(100);
dorian06 2:354d20277612 57 T1.attach(&calculvitesse, Temps);
vermaelen 0:94fe153be5b7 58
vermaelen 0:94fe153be5b7 59
dorian06 2:354d20277612 60 while(1) { //Boucle infinie
dorian06 2:354d20277612 61 int BpG;
dorian06 2:354d20277612 62 /*myled1=1;
dorian06 2:354d20277612 63 myled2=0;
dorian06 2:354d20277612 64 myled3=0;
dorian06 2:354d20277612 65 myled4=0;
dorian06 2:354d20277612 66 wait(0.5);
dorian06 2:354d20277612 67 myled1=0;
dorian06 2:354d20277612 68 myled2=1;
dorian06 2:354d20277612 69 myled3=0;
dorian06 2:354d20277612 70 myled4=0;
dorian06 2:354d20277612 71 wait(0.5);
dorian06 2:354d20277612 72 myled1=0;
dorian06 2:354d20277612 73 myled2=0;
dorian06 2:354d20277612 74 myled3=1;
dorian06 2:354d20277612 75 myled4=0;
dorian06 2:354d20277612 76 wait(0.5);
dorian06 2:354d20277612 77 myled1=0;
dorian06 2:354d20277612 78 myled2=0;
dorian06 2:354d20277612 79 myled3=0;
dorian06 2:354d20277612 80 myled4=1;
dorian06 2:354d20277612 81 wait(0.5);*/
dorian06 2:354d20277612 82 BpG=Bp1.read();
dorian06 2:354d20277612 83 Bat=(Vbat.read()*100)/7.4; //valeur de la batterie
dorian06 2:354d20277612 84 /*printf("batt:%g \t",Bat);
dorian06 2:354d20277612 85 printf("Bg: %d \t Bd: %d \t",Bp1.read(),Bp2.read());
dorian06 2:354d20277612 86 printf("Cd: %.02f \t Cg:%.02f \t",capt1.read(),capt2.read());
dorian06 2:354d20277612 87 printf("EAg:%d \t EAd:%d \t",Encodeur_AG.read(),Encodeur_AD.read());
dorian06 2:354d20277612 88 printf("EBg:%d \t EBd:%d \n\r",Encodeur_BG.read(),Encodeur_BD.read());
dorian06 2:354d20277612 89 wait(0.5);*/
dorian06 2:354d20277612 90 if (ticker==1) {
dorian06 2:354d20277612 91 //printf("vitesse mot Gauche:%g\n\r",vitessemsAG);
dorian06 2:354d20277612 92 //printf("vitesse mot Droite:%g\n\r",vitessemsAD);
dorian06 2:354d20277612 93 ticker=0;
dorian06 2:354d20277612 94 }
dorian06 2:354d20277612 95 dist= srf.read();
dorian06 2:354d20277612 96 if(dist>voulu) {
dorian06 2:354d20277612 97 sens_vitesse(30,1);
dorian06 2:354d20277612 98 sens_vitesse(30,2);
dorian06 2:354d20277612 99 } else if(dist< voulu) {
dorian06 2:354d20277612 100 sens_vitesse(-30,1);
dorian06 2:354d20277612 101 sens_vitesse(-30,2);
dorian06 2:354d20277612 102 } else if((dist<23) && (dist>27)) {
dorian06 2:354d20277612 103 sens_vitesse(0,1);
dorian06 2:354d20277612 104 sens_vitesse(0,2);
dorian06 2:354d20277612 105 }
dorian06 2:354d20277612 106 }
dorian06 2:354d20277612 107
dorian06 2:354d20277612 108
dorian06 2:354d20277612 109 }
vermaelen 0:94fe153be5b7 110
dorian06 2:354d20277612 111 void sens_vitesse(int Periode,int mot)
dorian06 2:354d20277612 112 {
dorian06 2:354d20277612 113 int sens;
dorian06 2:354d20277612 114 if (Periode<=0) { // determination du sens 1 si peride inf a 0
dorian06 2:354d20277612 115 sens=1;
dorian06 2:354d20277612 116 Periode=abs(Periode); // valeurs abs de la periode donc en positif
dorian06 2:354d20277612 117 } else {
dorian06 2:354d20277612 118 sens=0;
dorian06 2:354d20277612 119 }
dorian06 2:354d20277612 120 if((mot==1)&&(sens==1)) {
dorian06 2:354d20277612 121 sens_gauche.write(1);
dorian06 2:354d20277612 122 mot1.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode
vermaelen 0:94fe153be5b7 123
dorian06 2:354d20277612 124 } else if((mot==1)&& (sens==0)) {
dorian06 2:354d20277612 125 sens_gauche.write(0);
dorian06 2:354d20277612 126 mot1.pulsewidth_us(Periode);
dorian06 2:354d20277612 127 }
dorian06 2:354d20277612 128
dorian06 2:354d20277612 129 if((mot==2)&&(sens==1)) {
dorian06 2:354d20277612 130 sens_droit.write(1);
dorian06 2:354d20277612 131 mot2.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode
dorian06 2:354d20277612 132 } else if((mot==2)&& (sens==0)) {
dorian06 2:354d20277612 133 sens_droit.write(0);
dorian06 2:354d20277612 134 mot2.pulsewidth_us(Periode);
vermaelen 0:94fe153be5b7 135 }
vermaelen 0:94fe153be5b7 136 }
dorian06 2:354d20277612 137
dorian06 2:354d20277612 138 void gestion_cpt_AG(void)
dorian06 2:354d20277612 139 {
dorian06 2:354d20277612 140 cpt1++; // incrementation de +1 a chaque passage
dorian06 2:354d20277612 141 }
dorian06 2:354d20277612 142 void gestion_cpt_AD(void)
dorian06 2:354d20277612 143 {
dorian06 2:354d20277612 144 cpt2++; // incrementation de +1 a chaque passage
dorian06 2:354d20277612 145 }
dorian06 2:354d20277612 146
dorian06 2:354d20277612 147 void calculvitesse(void)
dorian06 2:354d20277612 148 {
dorian06 2:354d20277612 149 vitessemsAG = (cpt1*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
dorian06 2:354d20277612 150 vitesseradsAG = vitessemsAG/rayon; // m/s div par rayon donc = rad/s
dorian06 2:354d20277612 151 vitessemsAD = (cpt2*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
dorian06 2:354d20277612 152 vitesseradsAD = vitessemsAD/rayon; // m/s div par rayon donc = rad/s
dorian06 2:354d20277612 153 ticker=1;
dorian06 2:354d20277612 154 cpt1=0; // remise du compteur a 0
dorian06 2:354d20277612 155 cpt2=0; // remise du compteur a 0
dorian06 2:354d20277612 156 }
dorian06 2:354d20277612 157
dorian06 2:354d20277612 158 /*void position(float voulu)
dorian06 2:354d20277612 159 {
dorian06 2:354d20277612 160 dist= srf.read();
dorian06 2:354d20277612 161 if(dist>voulu) {
dorian06 2:354d20277612 162 sens_vitesse(30,1);
dorian06 2:354d20277612 163 sens_vitesse(30,2);
dorian06 2:354d20277612 164 } else if(dist< voulu) {
dorian06 2:354d20277612 165 sens_vitesse(-30,1);
dorian06 2:354d20277612 166 sens_vitesse(-30,2);
dorian06 2:354d20277612 167 } else if((dist<23) && (dist>27)) {
dorian06 2:354d20277612 168 sens_vitesse(0,1);
dorian06 2:354d20277612 169 sens_vitesse(0,2);
dorian06 2:354d20277612 170 }
dorian06 2:354d20277612 171 }*/