Marco Friedmann
/
Quadrocopter2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Quadrocopter
by 
Chris Elsholz
Embed:
(wiki syntax)
Show/hide line numbers
messen.cpp
00001 #include "mbed.h" 00002 #include "stdio.h" 00003 00004 00005 00006 00007 00008 extern Serial pc(SERIAL_TX, SERIAL_RX); 00009 extern SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk 00010 extern DigitalOut ncs(PE_4); //ssel 00011 00012 extern AnalogIn potis_1 (PF_3); 00013 extern AnalogIn potis_2 (PF_10); 00014 extern AnalogIn potis_3 (PF_4); 00015 extern AnalogIn potis_4 (PF_5); 00016 00017 extern PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot 00018 extern PwmOut Motor2 (PC_9); // Weiß orange 00019 extern PwmOut Motor3 (PC_6); // Grau weiß 00020 extern PwmOut Motor4 (PB_9); // Blau braun 00021 // Gelb und Orange Vcc +5V 00022 // Gnd Rot 00023 00024 int n1, n2, n3, n4; 00025 int16_t high, low; 00026 00027 /**********************/ 00028 /*Initialisieren**Gyro*/ 00029 /**********************/ 00030 void initialisierung_gyro() 00031 { 00032 spi.format(8,0); 00033 spi.frequency(1000000); 00034 00035 ncs = 0; 00036 spi.write(0x6B); // Register 107 00037 spi.write(0x80); //Reset // Standby off 00038 ncs = 1; 00039 wait_ms(1000); 00040 00041 00042 ncs=0; 00043 spi.write(0x1A); //CONFIG write // DLPF_CFG // Register 26 00044 spi.write(0x06); //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz 00045 ncs = 1; 00046 wait_ms(1); 00047 00048 ncs=0; 00049 spi.write(0x1B); //Gyro_CONFIG write 00050 spi.write(0x18); //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps 00051 ncs = 1; 00052 wait_ms(1); 00053 00054 ncs = 0; 00055 spi.write(0x17); // Register 23 00056 spi.write(0x00); // Offset High Byte Z 00057 ncs = 1; 00058 wait_ms(1); 00059 00060 ncs = 0; 00061 spi.write(0x18); // Register 24 00062 spi.write(0x17); // Offset Low Byte Z 00063 ncs = 1; 00064 wait_ms(1000); 00065 } 00066 00067 /**********************/ 00068 /*Initialisieren Acc */ 00069 /**********************/ 00070 00071 int16_t initialisierung_acc () 00072 { 00073 int i,faktor = 0x00; 00074 for(i=0;i<=2;i++) 00075 { 00076 ncs=0; 00077 spi.write(0x1c); //ACC_CONFIG write 00078 spi.write(faktor); //Skalierung 00=2g;08=4g;10=8g;18=16g 00079 ncs=1; //Teilung 16384;8192;4096;2048 00080 wait_us(0.1); 00081 00082 ncs=0; 00083 spi.write(0x1d); //ACC_CONFIG_2 08=460Hz;09=184Hz;0a=92Hz 00084 spi.write(0x0e); //TP-Filter 0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz 00085 ncs=1; 00086 wait_us(0.1); 00087 } 00088 switch (faktor) 00089 { 00090 case 0x00: faktor = 16384; break; 00091 case 0x08: faktor = 8192; break; 00092 case 0x10: faktor = 4096; break; 00093 case 0x18: faktor = 2048; break; 00094 } 00095 return faktor; 00096 } 00097 00098 00099 /*******************/ 00100 /*Messen Sensor roh*/ 00101 /*******************/ 00102 00103 void aktuell_roh(int16_t *z_g, int16_t *x_g, int16_t *y_g, int16_t *z_a, int16_t *x_a, int16_t *y_a) 00104 { 00105 ncs = 0; 00106 spi.write(0xc7); 00107 high = spi.write(0x0); 00108 ncs = 1; 00109 wait_us(0.1); 00110 ncs = 0; 00111 spi.write(0xc8); 00112 low = spi.write(0x0); 00113 ncs = 1; 00114 wait_us(0.1); 00115 (*z_g) = low | (high << 8); 00116 00117 ncs = 0; 00118 spi.write(0xc3); 00119 high = spi.write(0x0); 00120 ncs = 1; 00121 wait_us(1); 00122 ncs = 0; 00123 spi.write(0xc4); 00124 low = spi.write(0x0); 00125 ncs = 1; 00126 wait_us(0.1); 00127 (*x_g) = low | (high << 8); 00128 00129 ncs = 0; 00130 spi.write(0xc5); 00131 high = spi.write(0x0); 00132 ncs = 1; 00133 wait_us(0.1); 00134 ncs = 0; 00135 spi.write(0xc6); 00136 low = spi.write(0x0); 00137 ncs = 1; 00138 wait_us(0.1); 00139 (*y_g) = low | (high << 8); 00140 00141 ncs=0; 00142 spi.write(0xbb); 00143 high = spi.write(0x0); 00144 ncs=1; 00145 wait_us(0.1); 00146 ncs=0; 00147 spi.write(0xbc); 00148 low = spi.write(0x0); 00149 ncs=1; 00150 wait_us(0.1); 00151 (*x_a) = low | (high << 8); 00152 00153 ncs=0; 00154 spi.write(0xbd); 00155 high = spi.write(0x0); 00156 ncs=1; 00157 wait_us(0.1); 00158 ncs=0; 00159 spi.write(0xbe); 00160 low = spi.write(0x0); 00161 ncs=1; 00162 wait_us(0.1); 00163 (*y_a) = low | (high << 8); 00164 00165 ncs=0; 00166 spi.write(0xbf); 00167 high = spi.write(0x0); 00168 ncs=1; 00169 wait_us(0.1); 00170 ncs=0; 00171 spi.write(0xc0); 00172 low = spi.write(0x0); 00173 ncs=1; 00174 wait_us(0.1); 00175 (*z_a) = low | (high << 8); 00176 } 00177 00178 00179 /***************/ 00180 /*Messen Gyro Z*/ 00181 /***************/ 00182 00183 int16_t aktuell_gyro_z() 00184 { 00185 ncs = 0; 00186 spi.write(0xc7); //Z_OUT_H 00187 high = spi.write(0x0); 00188 ncs = 1; 00189 wait_us(0.1); 00190 00191 ncs = 0; 00192 spi.write(0xc8); //Z_OUT_L 00193 low = spi.write(0x0); 00194 ncs = 1; 00195 wait_us(0.1); 00196 return (low | high << 8); 00197 } 00198 00199 /***************/ 00200 /*Messen Gyro X*/ 00201 /***************/ 00202 00203 int16_t aktuell_gyro_x() 00204 { 00205 ncs = 0; 00206 spi.write(0xc3); //Z_OUT_H 00207 high = spi.write(0x0); 00208 ncs = 1; 00209 wait_us(1); 00210 00211 ncs = 0; 00212 spi.write(0xc4); //Z_OUT_L 00213 low = spi.write(0x0); 00214 ncs = 1; 00215 wait_us(0.1); 00216 return low | high << 8;; 00217 } 00218 00219 /***************/ 00220 /*Messen Gyro Y*/ 00221 /***************/ 00222 00223 int16_t aktuell_gyro_y() 00224 { 00225 ncs = 0; 00226 spi.write(0xc5); //Z_OUT_H 00227 high = spi.write(0x0); 00228 ncs = 1; 00229 wait_us(0.1); 00230 00231 ncs = 0; 00232 spi.write(0xc6); //Z_OUT_L 00233 low = spi.write(0x0); 00234 ncs = 1; 00235 wait_us(0.1); 00236 return low | high << 8; 00237 } 00238 00239 /************/ 00240 /*Messen Acc*/ 00241 /************/ 00242 00243 int16_t aktuell_acc_x () 00244 { 00245 ncs=0; 00246 spi.write(0xbb); 00247 high = spi.write(0x0); 00248 ncs=1; 00249 wait_us(0.1); 00250 00251 ncs=0; 00252 spi.write(0xbc); 00253 low = spi.write(0x0); 00254 ncs=1; 00255 wait_us(0.1); 00256 return low | high<<8; 00257 } 00258 00259 int16_t aktuell_acc_y () 00260 { 00261 ncs=0; 00262 spi.write(0xbd); 00263 high = spi.write(0x0); 00264 ncs=1; 00265 wait_us(0.1); 00266 00267 ncs=0; 00268 spi.write(0xbe); 00269 low = spi.write(0x0); 00270 ncs=1; 00271 wait_us(0.1); 00272 return low | high<<8; 00273 } 00274 00275 int16_t aktuell_acc_z () 00276 { 00277 ncs=0; 00278 spi.write(0xbf); 00279 high = spi.write(0x0); 00280 ncs=1; 00281 wait_us(0.1); 00282 00283 ncs=0; 00284 spi.write(0xc0); 00285 low = spi.write(0x0); 00286 ncs=1; 00287 wait_us(0.1); 00288 return low | high<<8; 00289 } 00290 00291 00292 /**********/ 00293 /*Anlernen*/ 00294 /**********/ 00295 void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4) 00296 { 00297 int n1 = n2 = n3 = n4 = 1900; 00298 Motor1->pulsewidth_us(n1); 00299 Motor2->pulsewidth_us(n2); 00300 Motor3->pulsewidth_us(n3); 00301 Motor4->pulsewidth_us(n4); 00302 pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r"); 00303 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700)); 00304 while (!*taster4) 00305 { 00306 if (*taster1) 00307 { 00308 n1 = n2 =n3 = n4 = 700; 00309 } 00310 if (*taster2) 00311 { 00312 n1 = n2 = n3 = n4 =1900; 00313 } 00314 Motor1->pulsewidth_us(n1); 00315 Motor2->pulsewidth_us(n2); 00316 Motor3->pulsewidth_us(n3); 00317 Motor4->pulsewidth_us(n4); 00318 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700)); 00319 } 00320 } 00321 00322 /**********/ 00323 /*Rauschen*/ 00324 /**********/ 00325 void viberationen(AnalogOut *rauschen, PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster4) 00326 { 00327 (*rauschen) = 1; 00328 int n1 = n2 = n3 = n4 = 700; 00329 Motor1->pulsewidth_us(n1); 00330 Motor2->pulsewidth_us(n2); 00331 Motor3->pulsewidth_us(n3); 00332 Motor4->pulsewidth_us(n4); 00333 wait_ms(10000); 00334 while(!*taster4) 00335 { 00336 for(int i = 1; i <= 1000; i++) 00337 { 00338 float k = aktuell_acc_x()*aktuell_acc_x(); 00339 k = sqrt(k) * 0.0000438596491; 00340 pc.printf("Winkel:%2.5f\n\r",k); 00341 (*rauschen) = k; 00342 wait_ms(10); 00343 } 00344 n1 = n2 = n3 = n4 = 1400; 00345 Motor1->pulsewidth_us(n1); 00346 Motor2->pulsewidth_us(n2); 00347 Motor3->pulsewidth_us(n3); 00348 Motor4->pulsewidth_us(n4); 00349 for(int i = 1; i <= 3000; i++) 00350 { 00351 float k = aktuell_acc_x() * aktuell_acc_x(); 00352 k = sqrt(k) * 0.0000438596491; 00353 (*rauschen) = k; 00354 wait_ms(10); 00355 } 00356 (*rauschen) = 0; 00357 wait_ms(100000); 00358 } 00359 } 00360 /****************/ 00361 /*Offset****Gyro*/ 00362 /****************/ 00363 void offset_gyro(float *z_off, float *x_off, float *y_off) 00364 { 00365 float z_off1, z_off2; 00366 float x_off1, x_off2; 00367 float y_off1, y_off2; 00368 float i; 00369 z_off2 = 1; 00370 x_off2 = 1; 00371 y_off2 = 1; 00372 z_off2 = 1; 00373 do 00374 { 00375 for(i = 1; i <= 5000; i++) 00376 { 00377 if ((z_off2 > 0.05) || (z_off2 < -0.05)) 00378 { 00379 z_off1 += aktuell_gyro_z(); 00380 } 00381 if ((y_off2 > 0.05) || (y_off2 < -0.05)) 00382 { 00383 y_off1 += aktuell_gyro_y(); 00384 } 00385 if ((x_off2 > 0.05) || (x_off2 < -0.05)) 00386 { 00387 x_off1 += aktuell_gyro_x(); 00388 } 00389 wait_ms(1); 00390 } 00391 if ((z_off2 > 0.05) || (z_off2 < -0.05)) 00392 { 00393 z_off1 /= i; 00394 } 00395 if ((y_off2 > 0.05) || (y_off2 < -0.05)) 00396 { 00397 y_off1 /= i; 00398 } 00399 if ((x_off2 > 0.05) || (x_off2 < -0.05)) 00400 { 00401 x_off1 /= i; 00402 } 00403 for(i = 1; i <= 5000; i++) 00404 { 00405 if ((z_off2 > 0.05) || (z_off2 < -0.05)) 00406 { 00407 z_off2 += aktuell_gyro_z() - z_off1; 00408 } 00409 if ((y_off2 > 0.05) || (y_off2 < -0.05)) 00410 { 00411 y_off2 += aktuell_gyro_y() - y_off1; 00412 } 00413 if ((x_off2 > 0.05) || (x_off2 < -0.05)) 00414 { 00415 x_off2 += aktuell_gyro_x() - x_off1; 00416 } 00417 wait_ms(1); 00418 } 00419 if ((z_off2 > 0.05) || (z_off2 < -0.05)) 00420 { 00421 z_off2 /= i; 00422 } 00423 if ((y_off2 > 0.05) || (y_off2 < -0.05)) 00424 { 00425 y_off2 /= i; 00426 } 00427 if ((x_off2 > 0.05) || (x_off2 < -0.05)) 00428 { 00429 x_off2 /= i; 00430 } 00431 } while(((z_off2 > 0.05) || (z_off2 < -0.05)) || ((y_off2 > 0.05) || (y_off2 < -0.05)) || ((x_off2 > 0.05) || (x_off2 < -0.05))); 00432 (*z_off) = z_off1 + z_off2; 00433 (*y_off) = y_off1 + y_off2; 00434 (*x_off) = x_off1 + x_off2; 00435 } 00436 00437 /****************/ 00438 /*Drift*****Gyro*/ 00439 /****************/ 00440 void drift_gyro(float *drift_z, float *drift_x, float *drift_y, Timer *timer, Timer *timer2, double *gain_g, double *roll_g, double *pitch_g, float *z_off, float *x_off, float *y_off) 00441 { 00442 timer->stop(); 00443 timer2->stop(); 00444 timer->reset(); 00445 timer2->reset(); 00446 timer->start(); 00447 timer2->start(); 00448 while(timer2->read_ms() <= 60000) 00449 { 00450 uint32_t zeit = timer->read_us(); 00451 timer->reset(); 00452 (*gain_g) = (*gain_g) + ((aktuell_gyro_z() - (*z_off)) * zeit * 0.000001 * 1/16.4); 00453 (*pitch_g) = (*pitch_g) + ((aktuell_gyro_y() - (*y_off)) * zeit * 0.000001 * 1/16.4); 00454 (*roll_g) = (*roll_g) + ((aktuell_gyro_x() - (*x_off)) * zeit * 0.000001 * 1/16.4); 00455 00456 } 00457 (*drift_z) = (*gain_g)/30000; //Drift alle 4ms 00458 (*drift_y) = (*pitch_g)/30000; //Drift alle 4ms 00459 (*drift_x) = (*roll_g)/30000; //Drift alle 4ms 00460 } 00461 00462 00463 void Motorsteurung(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster2, DigitalIn *taster3, DigitalIn *taster4, int *n1, int *n2, int *n3, int *n4) 00464 { 00465 static bool flanke1, hilfe1 = 0, flanke2, hilfe2 = 0, flanke3, hilfe3 = 0, flanke4, hilfe4 = 0; 00466 00467 flanke2 = *taster2; 00468 if ((flanke2 != 0) && (hilfe2 == 0)) 00469 { 00470 (*n1)+=50; 00471 (*n2)+=50; 00472 (*n3)+=50; 00473 (*n4)+=50; 00474 } 00475 hilfe2=flanke2; 00476 flanke3 = *taster3; 00477 if ((flanke3 != 0) && (hilfe3 == 0)) 00478 { 00479 (*n1)-=50; 00480 (*n2)-=50; 00481 (*n3)-=50; 00482 (*n4)-=50; 00483 } 00484 hilfe3=flanke3; 00485 flanke4 = *taster4; 00486 if ((flanke4 != 0) && (hilfe4 == 0)) 00487 { 00488 (*n1)=(*n2)=(*n3)=(*n4)=650; 00489 } 00490 hilfe4=flanke4; 00491 Motor1->pulsewidth_us(*n1); 00492 Motor2->pulsewidth_us(*n2); 00493 Motor3->pulsewidth_us(*n3); 00494 Motor4->pulsewidth_us(*n4); 00495 }
Generated on Sun Aug 14 2022 07:38:34 by
1.7.2