![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
init
main.cpp@0:98d2bcd7ae29, 2017-06-09 (annotated)
- Committer:
- qj604184
- Date:
- Fri Jun 09 23:20:19 2017 +0000
- Revision:
- 0:98d2bcd7ae29
cachanNRC
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
qj604184 | 0:98d2bcd7ae29 | 1 | #include "mbed.h" |
qj604184 | 0:98d2bcd7ae29 | 2 | #include "fct.h" |
qj604184 | 0:98d2bcd7ae29 | 3 | #include "Pixy.h" |
qj604184 | 0:98d2bcd7ae29 | 4 | #include "PixyLink.h" |
qj604184 | 0:98d2bcd7ae29 | 5 | |
qj604184 | 0:98d2bcd7ae29 | 6 | #define V_max 25 |
qj604184 | 0:98d2bcd7ae29 | 7 | #define V_recherche 25 |
qj604184 | 0:98d2bcd7ae29 | 8 | #define K 0.1 |
qj604184 | 0:98d2bcd7ae29 | 9 | DigitalOut cs(p13); |
qj604184 | 0:98d2bcd7ae29 | 10 | DigitalIn bp(p14); |
qj604184 | 0:98d2bcd7ae29 | 11 | Serial pc (USBTX,USBRX); |
qj604184 | 0:98d2bcd7ae29 | 12 | |
qj604184 | 0:98d2bcd7ae29 | 13 | |
qj604184 | 0:98d2bcd7ae29 | 14 | |
qj604184 | 0:98d2bcd7ae29 | 15 | int main() |
qj604184 | 0:98d2bcd7ae29 | 16 | { |
qj604184 | 0:98d2bcd7ae29 | 17 | bp.mode(PullUp); |
qj604184 | 0:98d2bcd7ae29 | 18 | MotG.period(PERIOD); |
qj604184 | 0:98d2bcd7ae29 | 19 | MotD.period(PERIOD); |
qj604184 | 0:98d2bcd7ae29 | 20 | Vitmoteur(0.0,0.0); |
qj604184 | 0:98d2bcd7ae29 | 21 | Pixy pixy= Pixy(p28,p27); |
qj604184 | 0:98d2bcd7ae29 | 22 | pixy.setSerialOutput(&pc); |
qj604184 | 0:98d2bcd7ae29 | 23 | int j=0; |
qj604184 | 0:98d2bcd7ae29 | 24 | uint16_t blocks; |
qj604184 | 0:98d2bcd7ae29 | 25 | float taille,errorX; |
qj604184 | 0:98d2bcd7ae29 | 26 | pc.printf("alive"); |
qj604184 | 0:98d2bcd7ae29 | 27 | |
qj604184 | 0:98d2bcd7ae29 | 28 | |
qj604184 | 0:98d2bcd7ae29 | 29 | while(1) { |
qj604184 | 0:98d2bcd7ae29 | 30 | init();// recupere valeurs capteurs ligne blanche |
qj604184 | 0:98d2bcd7ae29 | 31 | pc.printf("capt1= %d capt2= %d\n\r",captL1,captL3); |
qj604184 | 0:98d2bcd7ae29 | 32 | wait(0.5); |
qj604184 | 0:98d2bcd7ae29 | 33 | if(captL1==0 && captL3==0) |
qj604184 | 0:98d2bcd7ae29 | 34 | { |
qj604184 | 0:98d2bcd7ae29 | 35 | Vitmoteur(20,20); |
qj604184 | 0:98d2bcd7ae29 | 36 | } |
qj604184 | 0:98d2bcd7ae29 | 37 | else{ |
qj604184 | 0:98d2bcd7ae29 | 38 | Vitmoteur(-20,-20); |
qj604184 | 0:98d2bcd7ae29 | 39 | wait(5); |
qj604184 | 0:98d2bcd7ae29 | 40 | } |
qj604184 | 0:98d2bcd7ae29 | 41 | |
qj604184 | 0:98d2bcd7ae29 | 42 | } |
qj604184 | 0:98d2bcd7ae29 | 43 | } |
qj604184 | 0:98d2bcd7ae29 | 44 | //printf("ok"); |
qj604184 | 0:98d2bcd7ae29 | 45 | |
qj604184 | 0:98d2bcd7ae29 | 46 | //MotD.pulsewidth(0.5*PERIOD); |
qj604184 | 0:98d2bcd7ae29 | 47 | //MotG.pulsewidth(0.5*PERIOD); |
qj604184 | 0:98d2bcd7ae29 | 48 | //MotD.pulsewidth(0.00005); |
qj604184 | 0:98d2bcd7ae29 | 49 | |
qj604184 | 0:98d2bcd7ae29 | 50 | /* blocks=pixy.getBlocks(); |
qj604184 | 0:98d2bcd7ae29 | 51 | if(blocks) { |
qj604184 | 0:98d2bcd7ae29 | 52 | taille = pixy.blocks[j].width*pixy.blocks[j].height; |
qj604184 | 0:98d2bcd7ae29 | 53 | errorX=(160-pixy.blocks[j].x); |
qj604184 | 0:98d2bcd7ae29 | 54 | Vitmoteur(V_max-K*errorX,V_max+K*errorX); |
qj604184 | 0:98d2bcd7ae29 | 55 | //pc.printf("ok"); |
qj604184 | 0:98d2bcd7ae29 | 56 | //Vitmoteur(1,-1); |
qj604184 | 0:98d2bcd7ae29 | 57 | } else { |
qj604184 | 0:98d2bcd7ae29 | 58 | //pc.printf("beug"); |
qj604184 | 0:98d2bcd7ae29 | 59 | Vitmoteur(0.0,0.0); |
qj604184 | 0:98d2bcd7ae29 | 60 | } |
qj604184 | 0:98d2bcd7ae29 | 61 | |
qj604184 | 0:98d2bcd7ae29 | 62 | if(errorX>V_max) { |
qj604184 | 0:98d2bcd7ae29 | 63 | errorX=V_max; |
qj604184 | 0:98d2bcd7ae29 | 64 | } |
qj604184 | 0:98d2bcd7ae29 | 65 | if(errorX<-V_max) { |
qj604184 | 0:98d2bcd7ae29 | 66 | errorX=-V_max; |
qj604184 | 0:98d2bcd7ae29 | 67 | } |
qj604184 | 0:98d2bcd7ae29 | 68 | Vitmoteur(V_max-K*errorX,V_max+K*errorX); |
qj604184 | 0:98d2bcd7ae29 | 69 | |
qj604184 | 0:98d2bcd7ae29 | 70 | }*/ |
qj604184 | 0:98d2bcd7ae29 | 71 | |
qj604184 | 0:98d2bcd7ae29 | 72 | |
qj604184 | 0:98d2bcd7ae29 | 73 |