hachkathon
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- sedid
- Date:
- 2018-12-14
- Revision:
- 0:4effd7a50c67
File content as of revision 0:4effd7a50c67:
#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; } } }