Alexandre Pirotte
,,,,,,,,,,,,
Dependencies: CMPS03 SRF05 mbed pixy
Fork of
0000Non_stop_code_v1
by 
Alexandre Pirotte
Revision 0:a8cee96c9250, committed 2017-06-10
- Comitter:
- pirottealex
- Date:
- Sat Jun 10 04:58:21 2017 +0000
- Child:
- 1:99f469d63b9a
- Commit message:
- rrrr
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CMPS03.lib Sat Jun 10 04:58:21 2017 +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 Sat Jun 10 04:58:21 2017 +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 Sat Jun 10 04:58:21 2017 +0000
@@ -0,0 +1,53 @@
+#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_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 Sat Jun 10 04:58:21 2017 +0000 @@ -0,0 +1,62 @@ +#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; +extern int j,etat,bout; +extern uint16_t blocks; +extern float taille,errorX,cap_rot; +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 PwmOut Servo; +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 30 +#define V_recherche 18 +#define K 0.1 +#define V_moy 25 + +//PROTOTYPES +void lecture_blanc(void); +void vitmoteur(float,float); +void lecture_us(void); +void init(void); +void lecture_boussole(void); +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/globals.cpp Sat Jun 10 04:58:21 2017 +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); + +SPI spi(p5,p6,p7); + +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); +DigitalOut cs(p13); + + +int j=0,etat=0; +uint16_t blocks; +float taille,errorX,gBoussole,cap_set,cap_rot; +int captL1,bout; +int captL3; +float us_arriere; +Timer tempo; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jun 10 04:58:21 2017 +0000
@@ -0,0 +1,135 @@
+#include "mbed.h"
+#include "fct.h"
+#include "Pixy.h"
+#include "PixyLink.h"
+#include "SRF05.h"
+#include <CMPS03.h>
+
+Serial pc (USBTX,USBRX);
+int main()
+{
+ init();
+ Pixy pixy= Pixy(p28,p27);
+ pixy.setSerialOutput(&pc);
+ pc.printf("alive");
+
+ while(1) {
+ lecture_blanc(); // recupere valeurs capteurs ligne blanche
+ lecture_us(); // recupere valeurs capteurs us
+ lecture_boussole(); //gBoussole
+ bout=bp.read();
+ 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("sig: %d x: %d y: %d width: %d height: %d\n\r", pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
+ //pc.printf("OK");
+ }
+
+ switch(etat)
+ {
+ case(0) :
+
+ if(bout==0)
+ {
+ etat=100;
+ cap_set=gBoussole;
+ }
+ break;
+
+ case(100):
+
+ if(blocks)
+ {
+ etat=1;
+ }
+
+ else
+ {
+ etat=21;
+ }
+ break;
+
+ case(1) :
+
+ if(!blocks)
+ {
+ etat=2;
+ tempo.stop();
+ tempo.reset();
+ tempo.start();
+ }
+ break;
+ case(2) :
+
+ if(tempo.read()>0.5)
+ {
+ etat=3;
+ }
+ break;
+ case(3) :
+
+ if(us_arriere<25)
+ {
+ etat=100;
+ }
+ break;
+ case(21) :
+ cap_rot=cap_set+30;
+ if(cap_rot>360)
+ {
+ cap_rot=cap_rot-360;
+ }
+
+ if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
+ {
+ etat=22;
+ }
+ if(blocks)
+ {
+ etat=1;
+ }
+ break;
+ case(22) :
+ cap_rot=cap_set-30;
+ if(cap_rot<0)
+ {
+ cap_rot=cap_rot+360;
+ }
+
+ if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
+ {
+ etat=21;
+ }
+ if(blocks)
+ {
+ etat=1;
+ }
+ break;
+
+ }
+
+ switch(etat)
+ {
+ case(0) : vitmoteur(0,0);break;
+
+ case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
+
+ case(2) : vitmoteur(V_max,V_max);break;
+
+ case(3) : vitmoteur(-V_moy,-V_moy);break;
+
+ case(21) : vitmoteur(V_recherche,-V_recherche);break;
+
+ case(22) : vitmoteur(-V_recherche,V_recherche);break;
+
+ case(100) : break;
+ }
+
+ }
+}
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Jun 10 04:58:21 2017 +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 Sat Jun 10 04:58:21 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/pirottealex/code/pixy/#e7cb59aaf759