hachkathon
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
00001 #include "mbed.h" 00002 #include "CMPS03.h" 00003 #include "CNY70.h" 00004 #include "GP2A.h" 00005 #include "RC_Servo.h" 00006 #include "VMA306.h" 00007 #include "Pixy.h" 00008 #include "PID.h" 00009 00010 00011 #define PI 3.1415926535898 00012 00013 00014 #define VMOY 300 00015 #define VMOY2 120 00016 #define k 0.84 00017 #define ouvert 0.8 00018 #define fermer 1.0 00019 00020 Serial pc (PA_2, PA_3, 921600); 00021 PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); 00022 00023 CMPS03 boussole (PC_4); 00024 00025 CNY70 ligneD (PC_3); 00026 CNY70 ligneG (PC_2); 00027 CNY70 exterior (PA_7); 00028 00029 //GP2A sd1 (PB_0, 5, 80, 10); 00030 AnalogIn sd1(PB_0); 00031 00032 RC_Servo ballon (PB_10, 1); 00033 RC_Servo verrou (PA_15, 1); 00034 00035 VMA306 UltraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); 00036 00037 PIXY Pixy (PA_0, PA_1, 115200); 00038 00039 DigitalIn bp (PC_13); 00040 00041 00042 DigitalOut disquette (PA_12); 00043 00044 InterruptIn button1(USER_BUTTON); 00045 00046 Timer t1,t2; 00047 int gietat=0; 00048 double gCap=0; 00049 double gBoussole=0; 00050 double distance_petit=0; 00051 double erreur_cap; 00052 long taille_balle; 00053 void start() 00054 { 00055 gietat=1; 00056 gCap=boussole.getBearing(); 00057 t2.reset(); 00058 } 00059 00060 int main() 00061 { 00062 00063 int position_balle=0; 00064 int nbCC, nbNM; 00065 T_pixyCCBloc CCBuf; 00066 T_pixyNMBloc NMBuf; 00067 disquette = 0; 00068 motor.resetPosition(); 00069 verrou.write(ouvert); 00070 button1.fall(&start); 00071 t1.start(); 00072 t2.start(); 00073 00074 while(1) { 00075 gBoussole=boussole.getBearing(); 00076 distance_petit=(10.00/(sd1.read()*3.30-0.50))-0.42; 00077 if(distance_petit>20.0)distance_petit=20; 00078 if(distance_petit<3.5)distance_petit=3.5; 00079 erreur_cap=gCap-gBoussole; 00080 //pc.printf("etat:%d \t erreur:%0.2f \t distance:%0.2fn\n\r",gietat,abs(erreur_cap),distance_petit); 00081 switch(gietat) { 00082 case 0: 00083 break; 00084 case 1: 00085 if (Pixy.checkNewImage()) { 00086 gietat=2; 00087 } 00088 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) { 00089 gietat=6; 00090 t1.reset(); 00091 } 00092 if(t2.read()>=85) gietat=8; 00093 break; 00094 case 2: 00095 if( distance_petit<=4.50 && distance_petit>=3.8) { 00096 gietat=30; 00097 t1.reset(); 00098 } 00099 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) { 00100 gietat=6; 00101 t1.reset(); 00102 } 00103 if(t2.read()>=85) gietat=8; 00104 break; 00105 00106 case 3: 00107 if(abs(erreur_cap)<=7.0) { 00108 gietat=4; 00109 verrou.write(ouvert); 00110 } 00111 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) { 00112 gietat=6; 00113 t1.reset(); 00114 } 00115 if(t2.read()>=85) gietat=8; 00116 break; 00117 case 4: 00118 if((exterior.getVoltage()<0.2) && (ligneG.getVoltage()<0.2) && (ligneD.getVoltage()<0.2)) { 00119 gietat=5; 00120 } 00121 if(t2.read()>=85) gietat=8; 00122 break; 00123 case 5: 00124 t1.reset(); 00125 gietat=6; 00126 break; 00127 case 6: 00128 if(t1.read()>=2) { 00129 gietat=7; 00130 t1.reset(); 00131 } 00132 if(t2.read()>=85) gietat=8; 00133 break; 00134 case 7: 00135 if (t1.read()>1.75) { 00136 gietat=70; 00137 t1.reset(); 00138 } 00139 if(t2.read()>=85) gietat=8; 00140 break; 00141 case 70: 00142 if (Pixy.checkNewImage()) { 00143 gietat=2; 00144 } 00145 if (t1.read()>0.75) { 00146 gietat=71; 00147 t1.reset(); 00148 } 00149 00150 if(t2.read()>=85) gietat=8; 00151 break; 00152 case 71: 00153 if (Pixy.checkNewImage()) { 00154 gietat=2; 00155 } 00156 if (t1.read()>0.75) { 00157 gietat=71; 00158 t1.reset(); 00159 } 00160 if(t2.read()>=85) gietat=8; 00161 break; 00162 case 30: 00163 if(t1.read()>0.75) { 00164 gietat=31; 00165 t1.reset(); 00166 } 00167 if(t2.read()>=85) gietat=8; 00168 break; 00169 case 31: 00170 if(t1.read()>0.2) { 00171 gietat=3; 00172 t1.reset(); 00173 } 00174 if(t2.read()>=85) gietat=8; 00175 break; 00176 } 00177 switch(gietat) { 00178 case 0: 00179 motor.setSpeed(0,0); 00180 break; 00181 case 1: 00182 motor.setSpeed(-200,200); 00183 break; 00184 case 2: 00185 Pixy.detectedObject (&nbNM, &nbCC); 00186 if (nbNM > 0) { 00187 NMBuf = Pixy.getNMBloc (); 00188 nbNM--; 00189 //pc.printf ("\rNM %4x : x=%5d, y=%5d - w=%5d, h=%5d\n", NMBuf.signature, NMBuf.x, NMBuf.y, NMBuf.width, NMBuf.height); 00190 position_balle=NMBuf.x-160; 00191 //pc.printf("position_balle:%d \n\r",position_balle); 00192 motor.setSpeed((int)((VMOY+(k*position_balle))),(int)((VMOY-(k*position_balle)))); 00193 } 00194 break; 00195 case 3: 00196 motor.setSpeed(-150,150); 00197 verrou.write(fermer); 00198 break; 00199 case 30: 00200 motor.setSpeed(25,25); 00201 break; 00202 case 31: 00203 motor.setSpeed(25,25); 00204 verrou.write(fermer); 00205 break; 00206 case 4: 00207 motor.setSpeed(600,600); 00208 break; 00209 case 5: 00210 motor.setSpeed(0,0); 00211 break; 00212 case 6: 00213 motor.setSpeed(-150,-150); 00214 break; 00215 case 7: 00216 motor.setSpeed(-150,150); 00217 break; 00218 case 8: 00219 motor.setSpeed(0,0); 00220 break; 00221 case 70: 00222 motor.setSpeed(-150,150); 00223 break; 00224 case 71: 00225 motor.setSpeed(150,-150); 00226 break; 00227 00228 } 00229 } 00230 }
Generated on Wed Jul 13 2022 19:48:40 by 1.7.2