mm

Dependencies:   CMPS03 SRF05 mbed pixy

Files at this revision

API Documentation at this revision

Comitter:
pirottealex
Date:
Thu Feb 08 19:35:15 2018 +0000
Commit message:
mm

Changed in this revision

CMPS03.lib Show annotated file Show diff for this revision Revisions of this file
SRF05.lib Show annotated file Show diff for this revision Revisions of this file
fct.cpp Show annotated file Show diff for this revision Revisions of this file
fct.h Show annotated file Show diff for this revision Revisions of this file
globals.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
pixy.lib Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CMPS03.lib	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/CMPS03/#c6bcc390612a
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/SRF05/#e758665e072c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.cpp	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "fct.h"
+#include "SRF05.h"
+
+void vitmoteur(float VitG, float VitD)
+{
+    if(VitG<0) {
+        VitG=-1*VitG;
+        cmdI2C=cmdI2C&0xfe; //passe le moteur gauche en marche arriere 00000001
+    } else {
+        cmdI2C=cmdI2C|0x01; // marche avant 11110111 mot gauche
+    }
+    if(VitD<0) {
+        VitD=-1*VitD;
+        cmdI2C=cmdI2C&0xfd; //passe le moteur gauche en marche arriere 00000100
+    } else {
+        cmdI2C=cmdI2C|0x02;//marche avant 11111011 mot droit
+    }
+    monI2C.write(ADR_PCF,&cmdI2C,1);
+    MotG.pulsewidth(((100-VitG)/100.0)*PERIOD);
+    MotD.pulsewidth(((100-VitD)/100.0)*PERIOD);
+}
+void lecture_blanc(void)
+{
+    if(C1.read()>0.5) {
+        captL1=0;
+    } else {
+        captL1=1;
+    }
+    if(C3.read()>0.5) {
+        captL3=0;
+    } else {
+        captL3=1;
+    }
+}
+
+void lecture_an(void)
+{
+    capt_av=SD_2.read()*3.3;
+    capt_av=(10/(capt_av-0.5))-0.42;
+    if(capt_av<0)
+    {
+        capt_av=80;
+    }
+
+    distance_av=a/(3.3*LD_2.read()-b);
+    if(distance_av>120 || distance_av<20)
+    {
+        distance_av=0;
+    }
+
+}
+
+
+
+void lecture_us(void)
+{
+    us_arriere=us_arr.read();
+}
+
+void init(void)
+{
+    bp.mode(PullUp);
+    MotG.period(PERIOD);
+    MotD.period(PERIOD);
+    //vitmoteur(0,0);
+}
+
+void lecture_boussole(void)
+{
+    gBoussole=Boussole.readBearing() / 10.0;
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.h	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,65 @@
+#ifndef FCT_H
+#define FCT_H
+#include "SRF05.h"
+#include <CMPS03.h>
+extern char cmdI2C ;// CS vbat x x x SensG SensD CS_G CD_D, sens moteur positif CS à 0.
+extern PwmOut MotD;
+extern PwmOut MotG;
+extern int captL1;
+extern int captL3;
+extern float us_arriere,gBoussole,cap_set,distance_av;
+extern int j,etat,bout,capt_balle;
+extern uint16_t blocks;
+extern float taille,errorX,cap_rot,capt_av;
+extern Timer tempo;
+//GLOBALES
+extern BusOut leds;
+
+extern Serial CamPixy;
+
+extern DigitalOut trig1;//US1
+extern InterruptIn echo1;
+extern DigitalOut trig2;//US2
+extern InterruptIn echo2;
+extern DigitalOut trig3;//US3
+extern InterruptIn echo3;
+extern SRF05 us_arr;
+extern I2C monI2C;
+extern DigitalOut cs;
+extern DigitalIn bp;
+extern AnalogIn SD_1; // capteur de distance courte droite
+extern AnalogIn SD_2; // capteur de distance courte gauche
+extern AnalogIn LD_1; // capteur de distance longue droite
+extern AnalogIn LD_2; // capteur de distance longue gauche
+extern CMPS03 Boussole;
+extern InterruptIn I_D;
+extern InterruptIn I_G;
+
+extern SPI spi;
+
+extern AnalogIn C1;// capteur de ligne blanche 1
+extern AnalogIn C3;// capteur de ligne blanche 3 
+//-- le capteur de ligne 2 est sur un MCP3201(spi) dont le CS est sur p13
+extern DigitalOut cs;
+
+
+
+//CONSTANTES
+#define ADR_PCF 0x70
+#define PERIOD 0.0001
+#define V_max 25
+#define V_recherche 25
+#define K 0.2
+#define V_moy 30
+#define a 59.6
+#define b 0.1
+
+//PROTOTYPES
+void lecture_balle(void);
+void lecture_blanc(void);
+void vitmoteur(float,float);
+void lecture_us(void);
+void init(void);
+void lecture_boussole(void);
+void lecture_an(void);
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.cpp	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,46 @@
+#include "mbed.h"
+#include "SRF05.h"
+#include <CMPS03.h>
+char cmdI2C=0xF3 ;// CS vbat x x x SensG SensD CS_G CD_D, sens moteur positif CS à 0.
+PwmOut MotD(p25);
+PwmOut MotG(p22);
+
+BusOut leds(LED1,LED2,LED3,LED4);
+
+/*DigitalOut trig1(p11);//US1
+InterruptIn echo1(p12);
+DigitalOut trig2(p8);//US2
+InterruptIn echo2(p24);
+DigitalOut trig3(p26);//US3
+InterruptIn echo3(p23);*/
+
+SRF05 us_arr(p8,p24);
+CMPS03 Boussole(p9,p10,0xC0);
+I2C monI2C(p9,p10);
+//PwmOut Servo(p21);
+
+AnalogIn SD_1(p19); // capteur de distance courte droite
+AnalogIn SD_2(p20); // capteur de distance courte gauche
+AnalogIn LD_1(p17); // capteur de distance longue droite
+AnalogIn LD_2(p18); // capteur de distance longue gauche
+
+InterruptIn I_D(p30);
+InterruptIn I_G(p29);
+
+
+AnalogIn C1(p15);// capteur de ligne blanche 1
+AnalogIn C3(p16);// capteur de ligne blanche 3 
+//-- le capteur de ligne 2 est sur un MCP3201(spi) dont le CS est sur p13
+
+DigitalIn bp(p14);
+
+
+
+int j=0,etat=0;
+uint16_t blocks;
+float taille,errorX,gBoussole,cap_set,cap_rot;
+int captL1,bout;
+int captL3,capt_balle;
+
+float us_arriere,capt_av,distance_av;
+Timer tempo;
\ No newline at end of file
--- /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);
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/0f02307a0877
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixy.lib	Thu Feb 08 19:35:15 2018 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/pirottealex/code/pixy/#4bc00ec184a8