christophe vermaelen / Mbed 2 deprecated _test_suivi_mur

...

Dependencies:   mbed

main.cpp@0:dcb865a03d57, 2017-05-28 (annotated)

Committer:
vermaelen
Date:
Sun May 28 14:44:24 2017 +0000
Revision:
0:dcb865a03d57
Child:
1:714fd6b732be
V1 - trig ind?pendants;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 0:dcb865a03d57 1 #include "mbed.h"
vermaelen 0:dcb865a03d57 2 #define PERIOD 0.0001
vermaelen 0:dcb865a03d57 3 #define VMOY 25
vermaelen 0:dcb865a03d57 4 #define VMAX 60
vermaelen 0:dcb865a03d57 5 #define Kp_E 0.0
vermaelen 0:dcb865a03d57 6 #define Kp_ecart 0.8
vermaelen 0:dcb865a03d57 7 #define Td_ecart 0.0
vermaelen 0:dcb865a03d57 8 #define Ti_ecart 1000.0
vermaelen 0:dcb865a03d57 9 #define limitmin 3
vermaelen 0:dcb865a03d57 10 #define limitmax 150
vermaelen 0:dcb865a03d57 11 #define Te 0.001
vermaelen 0:dcb865a03d57 12 #define Ti 2.0
vermaelen 0:dcb865a03d57 13 #define a 24.0
vermaelen 0:dcb865a03d57 14 #define b 0.1
vermaelen 0:dcb865a03d57 15
vermaelen 0:dcb865a03d57 16
vermaelen 0:dcb865a03d57 17 BusOut leds(LED1,LED2,LED3,LED4);
vermaelen 0:dcb865a03d57 18 DigitalOut trigger1(p14);
vermaelen 0:dcb865a03d57 19 DigitalOut trigger2(p16);
vermaelen 0:dcb865a03d57 20 DigitalOut trigger3(p18);
vermaelen 0:dcb865a03d57 21 InterruptIn echo(p11);
vermaelen 0:dcb865a03d57 22 AnalogIn AnaG(p17);
vermaelen 0:dcb865a03d57 23 AnalogIn AnaAV(p15);
vermaelen 0:dcb865a03d57 24 PwmOut MG(p21); //vitesse moteur gauche
vermaelen 0:dcb865a03d57 25 PwmOut MD(p24); //vitesse moteur droit
vermaelen 0:dcb865a03d57 26 DigitalOut sensMG(p23); // sens moteur gauche
vermaelen 0:dcb865a03d57 27 DigitalOut sensMD(p26); // sens moteur droit
vermaelen 0:dcb865a03d57 28 Timer temp,t;
vermaelen 0:dcb865a03d57 29 Ticker tic1,tic2;
vermaelen 0:dcb865a03d57 30
vermaelen 0:dcb865a03d57 31 //GLOBALES
vermaelen 0:dcb865a03d57 32 int drap=1;
vermaelen 0:dcb865a03d57 33 float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50;
vermaelen 0:dcb865a03d57 34 float E_av,E,iE=0;
vermaelen 0:dcb865a03d57 35 float cmdG=0,cmdD=0;
vermaelen 0:dcb865a03d57 36 int etat=0;
vermaelen 0:dcb865a03d57 37 float iecart=0,ecart_av,ecart;
vermaelen 0:dcb865a03d57 38
vermaelen 0:dcb865a03d57 39 //PROTOTYPES
vermaelen 0:dcb865a03d57 40 void asservissement();
vermaelen 0:dcb865a03d57 41 void fcttrig();
vermaelen 0:dcb865a03d57 42 float vitesse(float);
vermaelen 0:dcb865a03d57 43 void start();
vermaelen 0:dcb865a03d57 44 void stop();
vermaelen 0:dcb865a03d57 45 float vitesse(float);
vermaelen 0:dcb865a03d57 46 void mesAN();
vermaelen 0:dcb865a03d57 47
vermaelen 0:dcb865a03d57 48 int main()
vermaelen 0:dcb865a03d57 49 {
vermaelen 0:dcb865a03d57 50
vermaelen 0:dcb865a03d57 51 sensMG.write(1);
vermaelen 0:dcb865a03d57 52 sensMD.write(1);
vermaelen 0:dcb865a03d57 53 MG.period(PERIOD);
vermaelen 0:dcb865a03d57 54 MD.period(PERIOD);
vermaelen 0:dcb865a03d57 55 MG.pulsewidth(vitesse(0));
vermaelen 0:dcb865a03d57 56 MD.pulsewidth(vitesse(0));
vermaelen 0:dcb865a03d57 57 tic1.attach(&fcttrig,0.05);
vermaelen 0:dcb865a03d57 58 tic2.attach(&mesAN,0.01);
vermaelen 0:dcb865a03d57 59 echo.rise(&start);
vermaelen 0:dcb865a03d57 60 echo.fall(&stop);
vermaelen 0:dcb865a03d57 61 while(1) {
vermaelen 0:dcb865a03d57 62 printf("US1=%.0f US2=%.0f US3=%.0f -- AN1=%.0f AN2=%.0f \n\r",US1,US2,US3,AN1,AN2);
vermaelen 0:dcb865a03d57 63 wait(0.1);
vermaelen 0:dcb865a03d57 64 }
vermaelen 0:dcb865a03d57 65
vermaelen 0:dcb865a03d57 66 }
vermaelen 0:dcb865a03d57 67 void mesAN()
vermaelen 0:dcb865a03d57 68 {
vermaelen 0:dcb865a03d57 69 AN1_av=AN1;
vermaelen 0:dcb865a03d57 70 AN2_av=AN2;
vermaelen 0:dcb865a03d57 71 AN1=a/(3.3*AnaG.read()-b);
vermaelen 0:dcb865a03d57 72 AN2=a/(3.3*AnaAV.read()-b);
vermaelen 0:dcb865a03d57 73 /* if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
vermaelen 0:dcb865a03d57 74 AN1=AN1_av;
vermaelen 0:dcb865a03d57 75 }
vermaelen 0:dcb865a03d57 76 if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
vermaelen 0:dcb865a03d57 77 AN2=AN2_av;
vermaelen 0:dcb865a03d57 78 }
vermaelen 0:dcb865a03d57 79 */
vermaelen 0:dcb865a03d57 80 if(AN1<0||AN1>150)AN1=150;
vermaelen 0:dcb865a03d57 81 if(AN2<0||AN2>150)AN2=150;
vermaelen 0:dcb865a03d57 82 }
vermaelen 0:dcb865a03d57 83 void fcttrig()
vermaelen 0:dcb865a03d57 84 {
vermaelen 0:dcb865a03d57 85 switch(drap) {
vermaelen 0:dcb865a03d57 86 case 1 :
vermaelen 0:dcb865a03d57 87 trigger2.write(1);
vermaelen 0:dcb865a03d57 88 wait_us(10);
vermaelen 0:dcb865a03d57 89 trigger2.write(0);
vermaelen 0:dcb865a03d57 90 drap=2;
vermaelen 0:dcb865a03d57 91 break;
vermaelen 0:dcb865a03d57 92 case 2 :
vermaelen 0:dcb865a03d57 93 trigger3.write(1);
vermaelen 0:dcb865a03d57 94 wait_us(10);
vermaelen 0:dcb865a03d57 95 trigger3.write(0);
vermaelen 0:dcb865a03d57 96 drap=3;
vermaelen 0:dcb865a03d57 97 break;
vermaelen 0:dcb865a03d57 98 case 3 :
vermaelen 0:dcb865a03d57 99 trigger1.write(1);
vermaelen 0:dcb865a03d57 100 wait_us(10);
vermaelen 0:dcb865a03d57 101 trigger1.write(0);
vermaelen 0:dcb865a03d57 102 drap=1;
vermaelen 0:dcb865a03d57 103 break;
vermaelen 0:dcb865a03d57 104 }
vermaelen 0:dcb865a03d57 105
vermaelen 0:dcb865a03d57 106 }
vermaelen 0:dcb865a03d57 107 void start()
vermaelen 0:dcb865a03d57 108 {
vermaelen 0:dcb865a03d57 109 temp.reset();
vermaelen 0:dcb865a03d57 110 temp.start();
vermaelen 0:dcb865a03d57 111 }
vermaelen 0:dcb865a03d57 112 void stop()
vermaelen 0:dcb865a03d57 113 {
vermaelen 0:dcb865a03d57 114 temp.stop();
vermaelen 0:dcb865a03d57 115 switch(drap) {
vermaelen 0:dcb865a03d57 116 case 1 :
vermaelen 0:dcb865a03d57 117 US3_av=US3;
vermaelen 0:dcb865a03d57 118 US3=temp.read_us()/58.31;
vermaelen 0:dcb865a03d57 119 // if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
vermaelen 0:dcb865a03d57 120 // US3=US3_av;
vermaelen 0:dcb865a03d57 121 // }
vermaelen 0:dcb865a03d57 122 break;
vermaelen 0:dcb865a03d57 123 case 2 :
vermaelen 0:dcb865a03d57 124 US2_av=US2;
vermaelen 0:dcb865a03d57 125 US2=temp.read_us()/58.31;
vermaelen 0:dcb865a03d57 126 // if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
vermaelen 0:dcb865a03d57 127 // US2=US2_av;
vermaelen 0:dcb865a03d57 128 // }
vermaelen 0:dcb865a03d57 129 break;
vermaelen 0:dcb865a03d57 130 case 3 :
vermaelen 0:dcb865a03d57 131 US1_av=US1;
vermaelen 0:dcb865a03d57 132 US1=temp.read_us()/58.31;
vermaelen 0:dcb865a03d57 133 // if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
vermaelen 0:dcb865a03d57 134 // US1=US1_av;
vermaelen 0:dcb865a03d57 135 // }
vermaelen 0:dcb865a03d57 136 break;
vermaelen 0:dcb865a03d57 137 }
vermaelen 0:dcb865a03d57 138 }
vermaelen 0:dcb865a03d57 139 float vitesse(float vit)
vermaelen 0:dcb865a03d57 140 {
vermaelen 0:dcb865a03d57 141 if(vit<0) vit=0;
vermaelen 0:dcb865a03d57 142 if(vit>VMAX) vit=VMAX;
vermaelen 0:dcb865a03d57 143 return ((vit/100.0)*PERIOD);
vermaelen 0:dcb865a03d57 144 }