![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test de la boussole
main.cpp@1:714fd6b732be, 2017-05-28 (annotated)
- Committer:
- vermaelen
- Date:
- Sun May 28 14:44:51 2017 +0000
- Revision:
- 1:714fd6b732be
- Parent:
- 0:dcb865a03d57
- Child:
- 2:fd17a1d6c0a3
V2 Trig 1 et 3 simultan?s
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:714fd6b732be | 83 | |
vermaelen | 0:dcb865a03d57 | 84 | void fcttrig() |
vermaelen | 0:dcb865a03d57 | 85 | { |
vermaelen | 0:dcb865a03d57 | 86 | switch(drap) { |
vermaelen | 0:dcb865a03d57 | 87 | case 1 : |
vermaelen | 0:dcb865a03d57 | 88 | trigger2.write(1); |
vermaelen | 0:dcb865a03d57 | 89 | wait_us(10); |
vermaelen | 0:dcb865a03d57 | 90 | trigger2.write(0); |
vermaelen | 0:dcb865a03d57 | 91 | drap=2; |
vermaelen | 0:dcb865a03d57 | 92 | break; |
vermaelen | 0:dcb865a03d57 | 93 | case 2 : |
vermaelen | 0:dcb865a03d57 | 94 | trigger3.write(1); |
vermaelen | 0:dcb865a03d57 | 95 | wait_us(10); |
vermaelen | 0:dcb865a03d57 | 96 | trigger3.write(0); |
vermaelen | 0:dcb865a03d57 | 97 | drap=3; |
vermaelen | 0:dcb865a03d57 | 98 | break; |
vermaelen | 0:dcb865a03d57 | 99 | case 3 : |
vermaelen | 0:dcb865a03d57 | 100 | trigger1.write(1); |
vermaelen | 0:dcb865a03d57 | 101 | wait_us(10); |
vermaelen | 0:dcb865a03d57 | 102 | trigger1.write(0); |
vermaelen | 0:dcb865a03d57 | 103 | drap=1; |
vermaelen | 0:dcb865a03d57 | 104 | break; |
vermaelen | 0:dcb865a03d57 | 105 | } |
vermaelen | 0:dcb865a03d57 | 106 | |
vermaelen | 0:dcb865a03d57 | 107 | } |
vermaelen | 0:dcb865a03d57 | 108 | void start() |
vermaelen | 0:dcb865a03d57 | 109 | { |
vermaelen | 0:dcb865a03d57 | 110 | temp.reset(); |
vermaelen | 0:dcb865a03d57 | 111 | temp.start(); |
vermaelen | 0:dcb865a03d57 | 112 | } |
vermaelen | 0:dcb865a03d57 | 113 | void stop() |
vermaelen | 0:dcb865a03d57 | 114 | { |
vermaelen | 0:dcb865a03d57 | 115 | temp.stop(); |
vermaelen | 0:dcb865a03d57 | 116 | switch(drap) { |
vermaelen | 0:dcb865a03d57 | 117 | case 1 : |
vermaelen | 0:dcb865a03d57 | 118 | US3_av=US3; |
vermaelen | 0:dcb865a03d57 | 119 | US3=temp.read_us()/58.31; |
vermaelen | 0:dcb865a03d57 | 120 | // if(((US3-US3_av)>50)||((US3-US3_av)<-50)) { |
vermaelen | 0:dcb865a03d57 | 121 | // US3=US3_av; |
vermaelen | 0:dcb865a03d57 | 122 | // } |
vermaelen | 0:dcb865a03d57 | 123 | break; |
vermaelen | 0:dcb865a03d57 | 124 | case 2 : |
vermaelen | 0:dcb865a03d57 | 125 | US2_av=US2; |
vermaelen | 0:dcb865a03d57 | 126 | US2=temp.read_us()/58.31; |
vermaelen | 0:dcb865a03d57 | 127 | // if(((US2-US2_av)>50)||((US2-US2_av)<-50)) { |
vermaelen | 0:dcb865a03d57 | 128 | // US2=US2_av; |
vermaelen | 0:dcb865a03d57 | 129 | // } |
vermaelen | 0:dcb865a03d57 | 130 | break; |
vermaelen | 0:dcb865a03d57 | 131 | case 3 : |
vermaelen | 0:dcb865a03d57 | 132 | US1_av=US1; |
vermaelen | 0:dcb865a03d57 | 133 | US1=temp.read_us()/58.31; |
vermaelen | 0:dcb865a03d57 | 134 | // if(((US1-US1_av)>50)||((US1-US1_av)<-50)) { |
vermaelen | 0:dcb865a03d57 | 135 | // US1=US1_av; |
vermaelen | 0:dcb865a03d57 | 136 | // } |
vermaelen | 0:dcb865a03d57 | 137 | break; |
vermaelen | 0:dcb865a03d57 | 138 | } |
vermaelen | 0:dcb865a03d57 | 139 | } |
vermaelen | 0:dcb865a03d57 | 140 | float vitesse(float vit) |
vermaelen | 0:dcb865a03d57 | 141 | { |
vermaelen | 0:dcb865a03d57 | 142 | if(vit<0) vit=0; |
vermaelen | 0:dcb865a03d57 | 143 | if(vit>VMAX) vit=VMAX; |
vermaelen | 0:dcb865a03d57 | 144 | return ((vit/100.0)*PERIOD); |
vermaelen | 0:dcb865a03d57 | 145 | } |