mm
Dependencies: CMPS03 SRF05 mbed pixy
main.cpp@0:6c5fac591b01, 2018-02-08 (annotated)
- Committer:
- pirottealex
- Date:
- Thu Feb 08 19:35:15 2018 +0000
- Revision:
- 0:6c5fac591b01
mm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pirottealex | 0:6c5fac591b01 | 1 | #include "mbed.h" |
pirottealex | 0:6c5fac591b01 | 2 | #include "fct.h" |
pirottealex | 0:6c5fac591b01 | 3 | #include "Pixy.h" |
pirottealex | 0:6c5fac591b01 | 4 | #include "PixyLink.h" |
pirottealex | 0:6c5fac591b01 | 5 | #include "SRF05.h" |
pirottealex | 0:6c5fac591b01 | 6 | #include <CMPS03.h> |
pirottealex | 0:6c5fac591b01 | 7 | #define TIC_POSITION 0.0005 |
pirottealex | 0:6c5fac591b01 | 8 | #define PERIOD_SERVO 0.01999 |
pirottealex | 0:6c5fac591b01 | 9 | |
pirottealex | 0:6c5fac591b01 | 10 | Serial pc (USBTX,USBRX); |
pirottealex | 0:6c5fac591b01 | 11 | //DigitalOut Servo(p21); |
pirottealex | 0:6c5fac591b01 | 12 | SPI spi(p5, p6, p7); // mosi, miso, sclk |
pirottealex | 0:6c5fac591b01 | 13 | DigitalOut cs(p13); |
pirottealex | 0:6c5fac591b01 | 14 | //Ticker tic1; |
pirottealex | 0:6c5fac591b01 | 15 | //Timer t_servo; |
pirottealex | 0:6c5fac591b01 | 16 | //int POSITION=50; |
pirottealex | 0:6c5fac591b01 | 17 | /*void servo_position( void) |
pirottealex | 0:6c5fac591b01 | 18 | { |
pirottealex | 0:6c5fac591b01 | 19 | float t = t_servo.read(); |
pirottealex | 0:6c5fac591b01 | 20 | if(t>=PERIOD_SERVO) |
pirottealex | 0:6c5fac591b01 | 21 | { |
pirottealex | 0:6c5fac591b01 | 22 | t_servo.reset(); |
pirottealex | 0:6c5fac591b01 | 23 | } |
pirottealex | 0:6c5fac591b01 | 24 | if(t>=0.001+(POSITION /100000.0)) |
pirottealex | 0:6c5fac591b01 | 25 | { |
pirottealex | 0:6c5fac591b01 | 26 | Servo.write(0); |
pirottealex | 0:6c5fac591b01 | 27 | } |
pirottealex | 0:6c5fac591b01 | 28 | else |
pirottealex | 0:6c5fac591b01 | 29 | { |
pirottealex | 0:6c5fac591b01 | 30 | Servo.write(1); |
pirottealex | 0:6c5fac591b01 | 31 | } |
pirottealex | 0:6c5fac591b01 | 32 | }*/ |
pirottealex | 0:6c5fac591b01 | 33 | |
pirottealex | 0:6c5fac591b01 | 34 | void mafct(void); |
pirottealex | 0:6c5fac591b01 | 35 | int main() |
pirottealex | 0:6c5fac591b01 | 36 | { |
pirottealex | 0:6c5fac591b01 | 37 | cs.write(1); //initialisation de CS à ‘1’ |
pirottealex | 0:6c5fac591b01 | 38 | spi.format(16,0); //communication sur 16 bits / mode 0 |
pirottealex | 0:6c5fac591b01 | 39 | spi.frequency(100000); //fréquence de 1MHz |
pirottealex | 0:6c5fac591b01 | 40 | init(); |
pirottealex | 0:6c5fac591b01 | 41 | //Pixy pixy= Pixy(p28,p27); |
pirottealex | 0:6c5fac591b01 | 42 | //pixy.setSerialOutput(&pc); |
pirottealex | 0:6c5fac591b01 | 43 | mafct(); |
pirottealex | 0:6c5fac591b01 | 44 | pc.baud(9600); |
pirottealex | 0:6c5fac591b01 | 45 | pc.printf("alive"); |
pirottealex | 0:6c5fac591b01 | 46 | etat=1; |
pirottealex | 0:6c5fac591b01 | 47 | for(int j=0; j<50;j++) |
pirottealex | 0:6c5fac591b01 | 48 | { |
pirottealex | 0:6c5fac591b01 | 49 | mafct(); |
pirottealex | 0:6c5fac591b01 | 50 | } |
pirottealex | 0:6c5fac591b01 | 51 | vitmoteur(0,0); |
pirottealex | 0:6c5fac591b01 | 52 | /*t_servo.start(); |
pirottealex | 0:6c5fac591b01 | 53 | tic1.attach(&servo_position,TIC_POSITION);*/ |
pirottealex | 0:6c5fac591b01 | 54 | while(1) { |
pirottealex | 0:6c5fac591b01 | 55 | mafct(); |
pirottealex | 0:6c5fac591b01 | 56 | /*lecture_blanc(); // recupere valeurs capteurs ligne blanche |
pirottealex | 0:6c5fac591b01 | 57 | lecture_us(); // recupere valeurs capteurs us |
pirottealex | 0:6c5fac591b01 | 58 | lecture_boussole(); //gBoussole |
pirottealex | 0:6c5fac591b01 | 59 | bout=bp.read(); |
pirottealex | 0:6c5fac591b01 | 60 | lecture_an();*/ |
pirottealex | 0:6c5fac591b01 | 61 | |
pirottealex | 0:6c5fac591b01 | 62 | /* blocks = pixy.getBlocks(); |
pirottealex | 0:6c5fac591b01 | 63 | //pc.printf("etat=%d\n\r",etat); |
pirottealex | 0:6c5fac591b01 | 64 | //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole); |
pirottealex | 0:6c5fac591b01 | 65 | if (blocks) { |
pirottealex | 0:6c5fac591b01 | 66 | taille=pixy.blocks[j].width*pixy.blocks[j].height; |
pirottealex | 0:6c5fac591b01 | 67 | errorX = (160-pixy.blocks[j].x); |
pirottealex | 0:6c5fac591b01 | 68 | //pc.printf("taille=%f,sig: %d x: %d y: %d width: %d height: %d\n\r",taille, pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height); |
pirottealex | 0:6c5fac591b01 | 69 | //pc.printf("OK"); |
pirottealex | 0:6c5fac591b01 | 70 | }*/ |
pirottealex | 0:6c5fac591b01 | 71 | //pc.printf("error=%0.2f us arriere : %0.2f capt_av=%0.2f capt_balle=%d distance_av=%0.2f cap1:%d cap2:%d boussole:%0.2f bout:%d\n\r",errorX, us_arriere,capt_av,capt_balle,distance_av, captL1,captL3,gBoussole,bout); |
pirottealex | 0:6c5fac591b01 | 72 | //pc.printf("%d\n\r",capt_balle); |
pirottealex | 0:6c5fac591b01 | 73 | //vitmoteur(V_moy+K*errorX,V_moy-K*errorX); |
pirottealex | 0:6c5fac591b01 | 74 | /* if(capt_av<4) |
pirottealex | 0:6c5fac591b01 | 75 | { |
pirottealex | 0:6c5fac591b01 | 76 | POSITION=-35; |
pirottealex | 0:6c5fac591b01 | 77 | } |
pirottealex | 0:6c5fac591b01 | 78 | else |
pirottealex | 0:6c5fac591b01 | 79 | { |
pirottealex | 0:6c5fac591b01 | 80 | POSITION=100; |
pirottealex | 0:6c5fac591b01 | 81 | }*/ |
pirottealex | 0:6c5fac591b01 | 82 | //vitmoteur(0,0); |
pirottealex | 0:6c5fac591b01 | 83 | |
pirottealex | 0:6c5fac591b01 | 84 | |
pirottealex | 0:6c5fac591b01 | 85 | } |
pirottealex | 0:6c5fac591b01 | 86 | } |
pirottealex | 0:6c5fac591b01 | 87 | |
pirottealex | 0:6c5fac591b01 | 88 | void mafct(void) |
pirottealex | 0:6c5fac591b01 | 89 | { |
pirottealex | 0:6c5fac591b01 | 90 | cs.write(0); |
pirottealex | 0:6c5fac591b01 | 91 | wait(0.01); //CS à ‘0’ <-> activation du circuit |
pirottealex | 0:6c5fac591b01 | 92 | unsigned int valeur = spi.write(0x00); //Lecture de la valeur en |
pirottealex | 0:6c5fac591b01 | 93 | //écrivant n’importe |
pirottealex | 0:6c5fac591b01 | 94 | //quoi... |
pirottealex | 0:6c5fac591b01 | 95 | valeur = (valeur>>1)&0x0FFF; //Mise en forme des deux |
pirottealex | 0:6c5fac591b01 | 96 | //bits lus et décalage de |
pirottealex | 0:6c5fac591b01 | 97 | //1bits pour ne pas tenir |
pirottealex | 0:6c5fac591b01 | 98 | //compte du deuxième B1 |
pirottealex | 0:6c5fac591b01 | 99 | pc.printf("valeur lue = %d\n\r", valeur); //Affichage sur la |
pirottealex | 0:6c5fac591b01 | 100 | //console |
pirottealex | 0:6c5fac591b01 | 101 | cs.write(1); //CS à ‘1’ <-> désactivation |
pirottealex | 0:6c5fac591b01 | 102 | wait(0.01); |
pirottealex | 0:6c5fac591b01 | 103 | } |
pirottealex | 0:6c5fac591b01 | 104 |