mm
Dependencies: CMPS03 SRF05 mbed pixy
Diff: main.cpp
- Revision:
- 0:6c5fac591b01
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Feb 08 19:35:15 2018 +0000 @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "fct.h" +#include "Pixy.h" +#include "PixyLink.h" +#include "SRF05.h" +#include <CMPS03.h> +#define TIC_POSITION 0.0005 +#define PERIOD_SERVO 0.01999 + +Serial pc (USBTX,USBRX); +//DigitalOut Servo(p21); +SPI spi(p5, p6, p7); // mosi, miso, sclk +DigitalOut cs(p13); +//Ticker tic1; +//Timer t_servo; +//int POSITION=50; +/*void servo_position( void) +{ + float t = t_servo.read(); + if(t>=PERIOD_SERVO) + { + t_servo.reset(); + } + if(t>=0.001+(POSITION /100000.0)) + { + Servo.write(0); + } + else + { + Servo.write(1); + } +}*/ + +void mafct(void); +int main() +{ + cs.write(1); //initialisation de CS à ‘1’ + spi.format(16,0); //communication sur 16 bits / mode 0 + spi.frequency(100000); //fréquence de 1MHz + init(); + //Pixy pixy= Pixy(p28,p27); + //pixy.setSerialOutput(&pc); + mafct(); + pc.baud(9600); + pc.printf("alive"); + etat=1; + for(int j=0; j<50;j++) + { + mafct(); + } + vitmoteur(0,0); + /*t_servo.start(); + tic1.attach(&servo_position,TIC_POSITION);*/ + while(1) { + mafct(); + /*lecture_blanc(); // recupere valeurs capteurs ligne blanche + lecture_us(); // recupere valeurs capteurs us + lecture_boussole(); //gBoussole + bout=bp.read(); + lecture_an();*/ + + /* blocks = pixy.getBlocks(); + //pc.printf("etat=%d\n\r",etat); + //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); + if (blocks) { + taille=pixy.blocks[j].width*pixy.blocks[j].height; + errorX = (160-pixy.blocks[j].x); + //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); + //pc.printf("OK"); + }*/ + //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); + //pc.printf("%d\n\r",capt_balle); + //vitmoteur(V_moy+K*errorX,V_moy-K*errorX); +/* if(capt_av<4) + { + POSITION=-35; + } + else + { + POSITION=100; + }*/ + //vitmoteur(0,0); + + + } +} + +void mafct(void) +{ + cs.write(0); + wait(0.01); //CS à ‘0’ <-> activation du circuit + unsigned int valeur = spi.write(0x00); //Lecture de la valeur en +//écrivant n’importe +//quoi... + valeur = (valeur>>1)&0x0FFF; //Mise en forme des deux +//bits lus et décalage de +//1bits pour ne pas tenir +//compte du deuxième B1 + pc.printf("valeur lue = %d\n\r", valeur); //Affichage sur la +//console + cs.write(1); //CS à ‘1’ <-> désactivation + wait(0.01); +} +