Dupuis sedid
hachkathon
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 0:4effd7a50c67
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Dec 14 12:27:28 2018 +0000
@@ -0,0 +1,230 @@
+#include "mbed.h"
+#include "CMPS03.h"
+#include "CNY70.h"
+#include "GP2A.h"
+#include "RC_Servo.h"
+#include "VMA306.h"
+#include "Pixy.h"
+#include "PID.h"
+
+
+#define PI 3.1415926535898
+
+
+#define VMOY 300
+#define VMOY2 120
+#define k 0.84
+#define ouvert 0.8
+#define fermer 1.0
+
+Serial pc (PA_2, PA_3, 921600);
+PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+
+CMPS03 boussole (PC_4);
+
+CNY70 ligneD (PC_3);
+CNY70 ligneG (PC_2);
+CNY70 exterior (PA_7);
+
+//GP2A sd1 (PB_0, 5, 80, 10);
+AnalogIn sd1(PB_0);
+
+RC_Servo ballon (PB_10, 1);
+RC_Servo verrou (PA_15, 1);
+
+VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
+
+PIXY Pixy (PA_0, PA_1, 115200);
+
+DigitalIn bp (PC_13);
+
+
+DigitalOut disquette (PA_12);
+
+InterruptIn button1(USER_BUTTON);
+
+Timer t1,t2;
+int gietat=0;
+double gCap=0;
+double gBoussole=0;
+double distance_petit=0;
+double erreur_cap;
+long taille_balle;
+void start()
+{
+ gietat=1;
+ gCap=boussole.getBearing();
+ t2.reset();
+}
+
+int main()
+{
+
+ int position_balle=0;
+ int nbCC, nbNM;
+ T_pixyCCBloc CCBuf;
+ T_pixyNMBloc NMBuf;
+ disquette = 0;
+ motor.resetPosition();
+ verrou.write(ouvert);
+ button1.fall(&start);
+ t1.start();
+ t2.start();
+
+ while(1) {
+ gBoussole=boussole.getBearing();
+ distance_petit=(10.00/(sd1.read()*3.30-0.50))-0.42;
+ if(distance_petit>20.0)distance_petit=20;
+ if(distance_petit<3.5)distance_petit=3.5;
+ erreur_cap=gCap-gBoussole;
+ //pc.printf("etat:%d \t erreur:%0.2f \t distance:%0.2fn\n\r",gietat,abs(erreur_cap),distance_petit);
+ switch(gietat) {
+ case 0:
+ break;
+ case 1:
+ if (Pixy.checkNewImage()) {
+ gietat=2;
+ }
+ if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
+ gietat=6;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 2:
+ if( distance_petit<=4.50 && distance_petit>=3.8) {
+ gietat=30;
+ t1.reset();
+ }
+ if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
+ gietat=6;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+
+ case 3:
+ if(abs(erreur_cap)<=7.0) {
+ gietat=4;
+ verrou.write(ouvert);
+ }
+ if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
+ gietat=6;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 4:
+ if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) {
+ gietat=5;
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 5:
+ t1.reset();
+ gietat=6;
+ break;
+ case 6:
+ if(t1.read()>=2) {
+ gietat=7;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 7:
+ if (t1.read()>1.75) {
+ gietat=70;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 70:
+ if (Pixy.checkNewImage()) {
+ gietat=2;
+ }
+ if (t1.read()>0.75) {
+ gietat=71;
+ t1.reset();
+ }
+
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 71:
+ if (Pixy.checkNewImage()) {
+ gietat=2;
+ }
+ if (t1.read()>0.75) {
+ gietat=71;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 30:
+ if(t1.read()>0.75) {
+ gietat=31;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ case 31:
+ if(t1.read()>0.2) {
+ gietat=3;
+ t1.reset();
+ }
+ if(t2.read()>=85) gietat=8;
+ break;
+ }
+ switch(gietat) {
+ case 0:
+ motor.setSpeed(0,0);
+ break;
+ case 1:
+ motor.setSpeed(-200,200);
+ break;
+ case 2:
+ Pixy.detectedObject (&nbNM, &nbCC);
+ if (nbNM > 0) {
+ NMBuf = Pixy.getNMBloc ();
+ nbNM--;
+ //pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height);
+ position_balle=NMBuf.x-160;
+ //pc.printf("position_balle:%d \n\r",position_balle);
+ motor.setSpeed((int)((VMOY+(k*position_balle))),(int)((VMOY-(k*position_balle))));
+ }
+ break;
+ case 3:
+ motor.setSpeed(-150,150);
+ verrou.write(fermer);
+ break;
+ case 30:
+ motor.setSpeed(25,25);
+ break;
+ case 31:
+ motor.setSpeed(25,25);
+ verrou.write(fermer);
+ break;
+ case 4:
+ motor.setSpeed(600,600);
+ break;
+ case 5:
+ motor.setSpeed(0,0);
+ break;
+ case 6:
+ motor.setSpeed(-150,-150);
+ break;
+ case 7:
+ motor.setSpeed(-150,150);
+ break;
+ case 8:
+ motor.setSpeed(0,0);
+ break;
+ case 70:
+ motor.setSpeed(-150,150);
+ break;
+ case 71:
+ motor.setSpeed(150,-150);
+ break;
+
+ }
+ }
+}