Alexandre Pirotte / Mbed 2 deprecated 0000Non_stop_code_v2

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?

UserRevisionLine numberNew 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