Chris Elsholz / Mbed 2 deprecated Quadrocopter

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.

Dependencies:   mbed TextLCD

Fork of Quadrocopter by Marco Friedmann

messen/messen.cpp@30:dc68b509f930, 2017-10-05 (annotated)

Committer:
chriselsholz
Date:
Thu Oct 05 18:02:40 2017 +0000
Revision:
30:dc68b509f930
Parent:
26:6c27486248fe 
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcoF89 4:3eaf38e4809f 1 #include "mbed.h"
MarcoF89 4:3eaf38e4809f 2 #include "stdio.h"
MarcoF89 4:3eaf38e4809f 3
MarcoF89 12:4a4dad7a3432 4
MarcoF89 15:742683a8efda 5
MarcoF89 15:742683a8efda 6
MarcoF89 4:3eaf38e4809f 7
MarcoF89 4:3eaf38e4809f 8 extern Serial pc(SERIAL_TX, SERIAL_RX);
MarcoF89 4:3eaf38e4809f 9 extern SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk
MarcoF89 4:3eaf38e4809f 10 extern DigitalOut ncs(PE_4); //ssel
MarcoF89 4:3eaf38e4809f 11
MarcoF89 7:a54c97795013 12 extern AnalogIn potis_1 (PF_3);
MarcoF89 7:a54c97795013 13 extern AnalogIn potis_2 (PF_10);
MarcoF89 7:a54c97795013 14 extern AnalogIn potis_3 (PF_4);
MarcoF89 7:a54c97795013 15 extern AnalogIn potis_4 (PF_5);
MarcoF89 7:a54c97795013 16
MarcoF89 12:4a4dad7a3432 17 extern PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot
MarcoF89 12:4a4dad7a3432 18 extern PwmOut Motor2 (PC_9); // Weiß orange
MarcoF89 12:4a4dad7a3432 19 extern PwmOut Motor3 (PC_6); // Grau weiß
MarcoF89 12:4a4dad7a3432 20 extern PwmOut Motor4 (PB_9); // Blau braun
MarcoF89 12:4a4dad7a3432 21 // Gelb und Orange Vcc +5V
MarcoF89 12:4a4dad7a3432 22 // Gnd Rot
MarcoF89 12:4a4dad7a3432 23
MarcoF89 15:742683a8efda 24 int n1, n2, n3, n4;
MarcoF89 15:742683a8efda 25 int16_t high, low;
MarcoF89 7:a54c97795013 26
chriselsholz 5:584acd257531 27 /**********************/
chriselsholz 5:584acd257531 28 /*Initialisieren**Gyro*/
chriselsholz 5:584acd257531 29 /**********************/
MarcoF89 12:4a4dad7a3432 30 void initialisierung_gyro()
chriselsholz 5:584acd257531 31 {
chriselsholz 5:584acd257531 32 spi.format(8,0);
chriselsholz 5:584acd257531 33 spi.frequency(1000000);
chriselsholz 5:584acd257531 34
chriselsholz 5:584acd257531 35 ncs = 0;
chriselsholz 5:584acd257531 36 spi.write(0x6B); // Register 107
chriselsholz 5:584acd257531 37 spi.write(0x80); //Reset // Standby off
chriselsholz 5:584acd257531 38 ncs = 1;
chriselsholz 5:584acd257531 39 wait_ms(1000);
chriselsholz 5:584acd257531 40
chriselsholz 5:584acd257531 41
chriselsholz 5:584acd257531 42 ncs=0;
chriselsholz 5:584acd257531 43 spi.write(0x1A); //CONFIG write // DLPF_CFG // Register 26
chriselsholz 5:584acd257531 44 spi.write(0x06); //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
chriselsholz 5:584acd257531 45 ncs = 1;
chriselsholz 5:584acd257531 46 wait_ms(1);
chriselsholz 5:584acd257531 47
chriselsholz 5:584acd257531 48 ncs=0;
chriselsholz 5:584acd257531 49 spi.write(0x1B); //Gyro_CONFIG write
chriselsholz 5:584acd257531 50 spi.write(0x18); //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
chriselsholz 5:584acd257531 51 ncs = 1;
chriselsholz 5:584acd257531 52 wait_ms(1);
chriselsholz 5:584acd257531 53
chriselsholz 5:584acd257531 54 ncs = 0;
chriselsholz 5:584acd257531 55 spi.write(0x17); // Register 23
MarcoF89 6:27a09e8bebfb 56 spi.write(0x00); // Offset High Byte Z
chriselsholz 5:584acd257531 57 ncs = 1;
chriselsholz 5:584acd257531 58 wait_ms(1);
chriselsholz 5:584acd257531 59
chriselsholz 5:584acd257531 60 ncs = 0;
chriselsholz 5:584acd257531 61 spi.write(0x18); // Register 24
MarcoF89 6:27a09e8bebfb 62 spi.write(0x17); // Offset Low Byte Z
chriselsholz 5:584acd257531 63 ncs = 1;
chriselsholz 5:584acd257531 64 wait_ms(1000);
chriselsholz 5:584acd257531 65 }
chriselsholz 5:584acd257531 66
chriselsholz 5:584acd257531 67 /**********************/
chriselsholz 5:584acd257531 68 /*Initialisieren Acc */
chriselsholz 5:584acd257531 69 /**********************/
chriselsholz 5:584acd257531 70
chriselsholz 5:584acd257531 71 int16_t initialisierung_acc ()
chriselsholz 5:584acd257531 72 {
chriselsholz 5:584acd257531 73 int i,faktor = 0x00;
chriselsholz 5:584acd257531 74 for(i=0;i<=2;i++)
chriselsholz 5:584acd257531 75 {
chriselsholz 5:584acd257531 76 ncs=0;
chriselsholz 5:584acd257531 77 spi.write(0x1c); //ACC_CONFIG write
chriselsholz 5:584acd257531 78 spi.write(faktor); //Skalierung 00=2g;08=4g;10=8g;18=16g
chriselsholz 5:584acd257531 79 ncs=1; //Teilung 16384;8192;4096;2048
chriselsholz 5:584acd257531 80 wait_us(0.1);
chriselsholz 5:584acd257531 81
chriselsholz 5:584acd257531 82 ncs=0;
chriselsholz 5:584acd257531 83 spi.write(0x1d); //ACC_CONFIG_2 08=460Hz;09=184Hz;0a=92Hz
MarcoF89 6:27a09e8bebfb 84 spi.write(0x0e); //TP-Filter 0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz
chriselsholz 5:584acd257531 85 ncs=1;
chriselsholz 5:584acd257531 86 wait_us(0.1);
chriselsholz 5:584acd257531 87 }
chriselsholz 5:584acd257531 88 switch (faktor)
chriselsholz 5:584acd257531 89 {
chriselsholz 14:a75b20f9cc24 90 case 0x00: faktor = 16384; break;
chriselsholz 14:a75b20f9cc24 91 case 0x08: faktor = 8192; break;
chriselsholz 14:a75b20f9cc24 92 case 0x10: faktor = 4096; break;
chriselsholz 14:a75b20f9cc24 93 case 0x18: faktor = 2048; break;
chriselsholz 5:584acd257531 94 }
chriselsholz 14:a75b20f9cc24 95 return faktor;
chriselsholz 5:584acd257531 96 }
chriselsholz 5:584acd257531 97
chriselsholz 5:584acd257531 98
MarcoF89 15:742683a8efda 99 /*******************/
MarcoF89 15:742683a8efda 100 /*Messen Sensor roh*/
MarcoF89 15:742683a8efda 101 /*******************/
MarcoF89 15:742683a8efda 102
MarcoF89 15:742683a8efda 103 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)
MarcoF89 15:742683a8efda 104 {
MarcoF89 15:742683a8efda 105 ncs = 0;
MarcoF89 15:742683a8efda 106 spi.write(0xc7);
MarcoF89 15:742683a8efda 107 high = spi.write(0x0);
MarcoF89 15:742683a8efda 108 ncs = 1;
MarcoF89 15:742683a8efda 109 wait_us(0.1);
MarcoF89 15:742683a8efda 110 ncs = 0;
MarcoF89 15:742683a8efda 111 spi.write(0xc8);
MarcoF89 15:742683a8efda 112 low = spi.write(0x0);
MarcoF89 15:742683a8efda 113 ncs = 1;
MarcoF89 15:742683a8efda 114 wait_us(0.1);
MarcoF89 15:742683a8efda 115 (*z_g) = low | (high << 8);
MarcoF89 15:742683a8efda 116
MarcoF89 15:742683a8efda 117 ncs = 0;
MarcoF89 15:742683a8efda 118 spi.write(0xc3);
MarcoF89 15:742683a8efda 119 high = spi.write(0x0);
MarcoF89 15:742683a8efda 120 ncs = 1;
MarcoF89 15:742683a8efda 121 wait_us(1);
MarcoF89 15:742683a8efda 122 ncs = 0;
MarcoF89 15:742683a8efda 123 spi.write(0xc4);
MarcoF89 15:742683a8efda 124 low = spi.write(0x0);
MarcoF89 15:742683a8efda 125 ncs = 1;
MarcoF89 15:742683a8efda 126 wait_us(0.1);
MarcoF89 15:742683a8efda 127 (*x_g) = low | (high << 8);
MarcoF89 15:742683a8efda 128
MarcoF89 15:742683a8efda 129 ncs = 0;
MarcoF89 15:742683a8efda 130 spi.write(0xc5);
MarcoF89 15:742683a8efda 131 high = spi.write(0x0);
MarcoF89 15:742683a8efda 132 ncs = 1;
MarcoF89 15:742683a8efda 133 wait_us(0.1);
MarcoF89 15:742683a8efda 134 ncs = 0;
MarcoF89 15:742683a8efda 135 spi.write(0xc6);
MarcoF89 15:742683a8efda 136 low = spi.write(0x0);
MarcoF89 15:742683a8efda 137 ncs = 1;
MarcoF89 15:742683a8efda 138 wait_us(0.1);
MarcoF89 15:742683a8efda 139 (*y_g) = low | (high << 8);
MarcoF89 15:742683a8efda 140
MarcoF89 15:742683a8efda 141 ncs=0;
MarcoF89 15:742683a8efda 142 spi.write(0xbb);
MarcoF89 15:742683a8efda 143 high = spi.write(0x0);
MarcoF89 15:742683a8efda 144 ncs=1;
MarcoF89 15:742683a8efda 145 wait_us(0.1);
MarcoF89 15:742683a8efda 146 ncs=0;
MarcoF89 15:742683a8efda 147 spi.write(0xbc);
MarcoF89 15:742683a8efda 148 low = spi.write(0x0);
MarcoF89 15:742683a8efda 149 ncs=1;
MarcoF89 15:742683a8efda 150 wait_us(0.1);
MarcoF89 15:742683a8efda 151 (*x_a) = low | (high << 8);
MarcoF89 15:742683a8efda 152
MarcoF89 15:742683a8efda 153 ncs=0;
MarcoF89 15:742683a8efda 154 spi.write(0xbd);
MarcoF89 15:742683a8efda 155 high = spi.write(0x0);
MarcoF89 15:742683a8efda 156 ncs=1;
MarcoF89 15:742683a8efda 157 wait_us(0.1);
MarcoF89 15:742683a8efda 158 ncs=0;
MarcoF89 15:742683a8efda 159 spi.write(0xbe);
MarcoF89 15:742683a8efda 160 low = spi.write(0x0);
MarcoF89 15:742683a8efda 161 ncs=1;
MarcoF89 15:742683a8efda 162 wait_us(0.1);
MarcoF89 15:742683a8efda 163 (*y_a) = low | (high << 8);
MarcoF89 15:742683a8efda 164
MarcoF89 15:742683a8efda 165 ncs=0;
MarcoF89 15:742683a8efda 166 spi.write(0xbf);
MarcoF89 15:742683a8efda 167 high = spi.write(0x0);
MarcoF89 15:742683a8efda 168 ncs=1;
MarcoF89 15:742683a8efda 169 wait_us(0.1);
MarcoF89 15:742683a8efda 170 ncs=0;
MarcoF89 15:742683a8efda 171 spi.write(0xc0);
MarcoF89 15:742683a8efda 172 low = spi.write(0x0);
MarcoF89 15:742683a8efda 173 ncs=1;
MarcoF89 15:742683a8efda 174 wait_us(0.1);
MarcoF89 15:742683a8efda 175 (*z_a) = low | (high << 8);
MarcoF89 15:742683a8efda 176 }
MarcoF89 15:742683a8efda 177
MarcoF89 15:742683a8efda 178
chriselsholz 5:584acd257531 179 /***************/
chriselsholz 5:584acd257531 180 /*Messen Gyro Z*/
chriselsholz 5:584acd257531 181 /***************/
MarcoF89 4:3eaf38e4809f 182
MarcoF89 6:27a09e8bebfb 183 int16_t aktuell_gyro_z()
MarcoF89 6:27a09e8bebfb 184 {
MarcoF89 4:3eaf38e4809f 185 ncs = 0;
MarcoF89 4:3eaf38e4809f 186 spi.write(0xc7); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 187 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 188 ncs = 1;
MarcoF89 6:27a09e8bebfb 189 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 190
MarcoF89 6:27a09e8bebfb 191 ncs = 0;
MarcoF89 6:27a09e8bebfb 192 spi.write(0xc8); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 193 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 194 ncs = 1;
MarcoF89 6:27a09e8bebfb 195 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 196 return (low | high << 8);
MarcoF89 6:27a09e8bebfb 197 }
MarcoF89 6:27a09e8bebfb 198
MarcoF89 6:27a09e8bebfb 199 /***************/
MarcoF89 6:27a09e8bebfb 200 /*Messen Gyro X*/
MarcoF89 6:27a09e8bebfb 201 /***************/
MarcoF89 6:27a09e8bebfb 202
MarcoF89 6:27a09e8bebfb 203 int16_t aktuell_gyro_x()
MarcoF89 6:27a09e8bebfb 204 {
MarcoF89 6:27a09e8bebfb 205 ncs = 0;
MarcoF89 6:27a09e8bebfb 206 spi.write(0xc3); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 207 high = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 208 ncs = 1;
MarcoF89 4:3eaf38e4809f 209 wait_us(1);
MarcoF89 4:3eaf38e4809f 210
MarcoF89 4:3eaf38e4809f 211 ncs = 0;
MarcoF89 6:27a09e8bebfb 212 spi.write(0xc4); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 213 low = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 214 ncs = 1;
MarcoF89 6:27a09e8bebfb 215 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 216 return low | high << 8;;
MarcoF89 6:27a09e8bebfb 217 }
MarcoF89 6:27a09e8bebfb 218
MarcoF89 6:27a09e8bebfb 219 /***************/
MarcoF89 6:27a09e8bebfb 220 /*Messen Gyro Y*/
MarcoF89 6:27a09e8bebfb 221 /***************/
MarcoF89 6:27a09e8bebfb 222
MarcoF89 6:27a09e8bebfb 223 int16_t aktuell_gyro_y()
MarcoF89 6:27a09e8bebfb 224 {
MarcoF89 6:27a09e8bebfb 225 ncs = 0;
MarcoF89 6:27a09e8bebfb 226 spi.write(0xc5); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 227 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 228 ncs = 1;
MarcoF89 6:27a09e8bebfb 229 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 230
MarcoF89 6:27a09e8bebfb 231 ncs = 0;
MarcoF89 6:27a09e8bebfb 232 spi.write(0xc6); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 233 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 234 ncs = 1;
MarcoF89 6:27a09e8bebfb 235 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 236 return low | high << 8;
MarcoF89 4:3eaf38e4809f 237 }
MarcoF89 4:3eaf38e4809f 238
chriselsholz 5:584acd257531 239 /************/
chriselsholz 5:584acd257531 240 /*Messen Acc*/
chriselsholz 5:584acd257531 241 /************/
MarcoF89 4:3eaf38e4809f 242
chriselsholz 5:584acd257531 243 int16_t aktuell_acc_x ()
chriselsholz 5:584acd257531 244 {
MarcoF89 4:3eaf38e4809f 245 ncs=0;
chriselsholz 5:584acd257531 246 spi.write(0xbb);
MarcoF89 6:27a09e8bebfb 247 high = spi.write(0x0);
chriselsholz 5:584acd257531 248 ncs=1;
chriselsholz 5:584acd257531 249 wait_us(0.1);
chriselsholz 5:584acd257531 250
chriselsholz 5:584acd257531 251 ncs=0;
chriselsholz 5:584acd257531 252 spi.write(0xbc);
MarcoF89 6:27a09e8bebfb 253 low = spi.write(0x0);
chriselsholz 5:584acd257531 254 ncs=1;
MarcoF89 6:27a09e8bebfb 255 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 256 return low | high<<8;
chriselsholz 5:584acd257531 257 }
MarcoF89 4:3eaf38e4809f 258
chriselsholz 5:584acd257531 259 int16_t aktuell_acc_y ()
chriselsholz 5:584acd257531 260 {
chriselsholz 5:584acd257531 261 ncs=0;
chriselsholz 5:584acd257531 262 spi.write(0xbd);
MarcoF89 6:27a09e8bebfb 263 high = spi.write(0x0);
chriselsholz 5:584acd257531 264 ncs=1;
chriselsholz 5:584acd257531 265 wait_us(0.1);
chriselsholz 5:584acd257531 266
chriselsholz 5:584acd257531 267 ncs=0;
chriselsholz 5:584acd257531 268 spi.write(0xbe);
MarcoF89 6:27a09e8bebfb 269 low = spi.write(0x0);
chriselsholz 5:584acd257531 270 ncs=1;
MarcoF89 6:27a09e8bebfb 271 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 272 return low | high<<8;
chriselsholz 5:584acd257531 273 }
MarcoF89 4:3eaf38e4809f 274
chriselsholz 5:584acd257531 275 int16_t aktuell_acc_z ()
chriselsholz 5:584acd257531 276 {
chriselsholz 5:584acd257531 277 ncs=0;
chriselsholz 5:584acd257531 278 spi.write(0xbf);
MarcoF89 6:27a09e8bebfb 279 high = spi.write(0x0);
chriselsholz 5:584acd257531 280 ncs=1;
chriselsholz 5:584acd257531 281 wait_us(0.1);
chriselsholz 5:584acd257531 282
chriselsholz 5:584acd257531 283 ncs=0;
chriselsholz 5:584acd257531 284 spi.write(0xc0);
MarcoF89 6:27a09e8bebfb 285 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 286 ncs=1;
MarcoF89 6:27a09e8bebfb 287 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 288 return low | high<<8;
chriselsholz 5:584acd257531 289 }
MarcoF89 12:4a4dad7a3432 290
chriselsholz 14:a75b20f9cc24 291
MarcoF89 13:5f0a2103c707 292 /**********/
MarcoF89 13:5f0a2103c707 293 /*Anlernen*/
MarcoF89 13:5f0a2103c707 294 /**********/
MarcoF89 13:5f0a2103c707 295 void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 296 {
MarcoF89 13:5f0a2103c707 297 int n1 = n2 = n3 = n4 = 1900;
MarcoF89 13:5f0a2103c707 298 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 299 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 300 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 301 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 302 pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r");
MarcoF89 13:5f0a2103c707 303 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 304 while (!*taster4)
MarcoF89 13:5f0a2103c707 305 {
MarcoF89 13:5f0a2103c707 306 if (*taster1)
MarcoF89 13:5f0a2103c707 307 {
MarcoF89 13:5f0a2103c707 308 n1 = n2 =n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 309 }
MarcoF89 13:5f0a2103c707 310 if (*taster2)
MarcoF89 13:5f0a2103c707 311 {
MarcoF89 13:5f0a2103c707 312 n1 = n2 = n3 = n4 =1900;
MarcoF89 13:5f0a2103c707 313 }
MarcoF89 13:5f0a2103c707 314 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 315 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 316 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 317 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 318 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 319 }
MarcoF89 13:5f0a2103c707 320 }
MarcoF89 13:5f0a2103c707 321
MarcoF89 13:5f0a2103c707 322 /**********/
MarcoF89 13:5f0a2103c707 323 /*Rauschen*/
MarcoF89 13:5f0a2103c707 324 /**********/
MarcoF89 13:5f0a2103c707 325 void viberationen(AnalogOut *rauschen, PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 326 {
MarcoF89 13:5f0a2103c707 327 (*rauschen) = 1;
MarcoF89 13:5f0a2103c707 328 int n1 = n2 = n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 329 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 330 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 331 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 332 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 333 wait_ms(10000);
MarcoF89 13:5f0a2103c707 334 while(!*taster4)
MarcoF89 13:5f0a2103c707 335 {
MarcoF89 13:5f0a2103c707 336 for(int i = 1; i <= 1000; i++)
MarcoF89 13:5f0a2103c707 337 {
MarcoF89 13:5f0a2103c707 338 float k = aktuell_acc_x()*aktuell_acc_x();
MarcoF89 13:5f0a2103c707 339 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 340 pc.printf("Winkel:%2.5f\n\r",k);
MarcoF89 13:5f0a2103c707 341 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 342 wait_ms(10);
MarcoF89 13:5f0a2103c707 343 }
MarcoF89 13:5f0a2103c707 344 n1 = n2 = n3 = n4 = 1400;
MarcoF89 13:5f0a2103c707 345 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 346 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 347 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 348 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 349 for(int i = 1; i <= 3000; i++)
MarcoF89 13:5f0a2103c707 350 {
MarcoF89 13:5f0a2103c707 351 float k = aktuell_acc_x() * aktuell_acc_x();
MarcoF89 13:5f0a2103c707 352 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 353 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 354 wait_ms(10);
MarcoF89 13:5f0a2103c707 355 }
MarcoF89 13:5f0a2103c707 356 (*rauschen) = 0;
MarcoF89 13:5f0a2103c707 357 wait_ms(100000);
MarcoF89 13:5f0a2103c707 358 }
MarcoF89 15:742683a8efda 359 }
MarcoF89 15:742683a8efda 360 /****************/
MarcoF89 15:742683a8efda 361 /*Offset****Gyro*/
MarcoF89 15:742683a8efda 362 /****************/
MarcoF89 15:742683a8efda 363 void offset_gyro(float *z_off, float *x_off, float *y_off)
MarcoF89 15:742683a8efda 364 {
MarcoF89 15:742683a8efda 365 float z_off1, z_off2;
MarcoF89 15:742683a8efda 366 float x_off1, x_off2;
MarcoF89 15:742683a8efda 367 float y_off1, y_off2;
MarcoF89 15:742683a8efda 368 float i;
MarcoF89 15:742683a8efda 369 z_off2 = 1;
MarcoF89 15:742683a8efda 370 x_off2 = 1;
MarcoF89 15:742683a8efda 371 y_off2 = 1;
MarcoF89 15:742683a8efda 372 z_off2 = 1;
MarcoF89 15:742683a8efda 373 do
MarcoF89 15:742683a8efda 374 {
MarcoF89 15:742683a8efda 375 for(i = 1; i <= 5000; i++)
MarcoF89 15:742683a8efda 376 {
MarcoF89 15:742683a8efda 377 if ((z_off2 > 0.05) || (z_off2 < -0.05))
MarcoF89 15:742683a8efda 378 {
MarcoF89 15:742683a8efda 379 z_off1 += aktuell_gyro_z();
MarcoF89 15:742683a8efda 380 }
MarcoF89 15:742683a8efda 381 if ((y_off2 > 0.05) || (y_off2 < -0.05))
MarcoF89 15:742683a8efda 382 {
MarcoF89 15:742683a8efda 383 y_off1 += aktuell_gyro_y();
MarcoF89 15:742683a8efda 384 }
MarcoF89 15:742683a8efda 385 if ((x_off2 > 0.05) || (x_off2 < -0.05))
MarcoF89 15:742683a8efda 386 {
MarcoF89 15:742683a8efda 387 x_off1 += aktuell_gyro_x();
MarcoF89 15:742683a8efda 388 }
MarcoF89 15:742683a8efda 389 wait_ms(1);
MarcoF89 15:742683a8efda 390 }
MarcoF89 15:742683a8efda 391 if ((z_off2 > 0.05) || (z_off2 < -0.05))
MarcoF89 15:742683a8efda 392 {
MarcoF89 15:742683a8efda 393 z_off1 /= i;
MarcoF89 15:742683a8efda 394 }
MarcoF89 15:742683a8efda 395 if ((y_off2 > 0.05) || (y_off2 < -0.05))
MarcoF89 15:742683a8efda 396 {
MarcoF89 15:742683a8efda 397 y_off1 /= i;
MarcoF89 15:742683a8efda 398 }
MarcoF89 15:742683a8efda 399 if ((x_off2 > 0.05) || (x_off2 < -0.05))
MarcoF89 15:742683a8efda 400 {
MarcoF89 15:742683a8efda 401 x_off1 /= i;
MarcoF89 15:742683a8efda 402 }
MarcoF89 15:742683a8efda 403 for(i = 1; i <= 5000; i++)
MarcoF89 15:742683a8efda 404 {
MarcoF89 15:742683a8efda 405 if ((z_off2 > 0.05) || (z_off2 < -0.05))
MarcoF89 15:742683a8efda 406 {
MarcoF89 15:742683a8efda 407 z_off2 += aktuell_gyro_z() - z_off1;
MarcoF89 15:742683a8efda 408 }
MarcoF89 15:742683a8efda 409 if ((y_off2 > 0.05) || (y_off2 < -0.05))
MarcoF89 15:742683a8efda 410 {
MarcoF89 15:742683a8efda 411 y_off2 += aktuell_gyro_y() - y_off1;
MarcoF89 15:742683a8efda 412 }
MarcoF89 15:742683a8efda 413 if ((x_off2 > 0.05) || (x_off2 < -0.05))
MarcoF89 15:742683a8efda 414 {
MarcoF89 15:742683a8efda 415 x_off2 += aktuell_gyro_x() - x_off1;
MarcoF89 15:742683a8efda 416 }
MarcoF89 15:742683a8efda 417 wait_ms(1);
MarcoF89 15:742683a8efda 418 }
MarcoF89 15:742683a8efda 419 if ((z_off2 > 0.05) || (z_off2 < -0.05))
MarcoF89 15:742683a8efda 420 {
MarcoF89 15:742683a8efda 421 z_off2 /= i;
MarcoF89 15:742683a8efda 422 }
MarcoF89 15:742683a8efda 423 if ((y_off2 > 0.05) || (y_off2 < -0.05))
MarcoF89 15:742683a8efda 424 {
MarcoF89 15:742683a8efda 425 y_off2 /= i;
MarcoF89 15:742683a8efda 426 }
MarcoF89 15:742683a8efda 427 if ((x_off2 > 0.05) || (x_off2 < -0.05))
MarcoF89 15:742683a8efda 428 {
MarcoF89 15:742683a8efda 429 x_off2 /= i;
MarcoF89 15:742683a8efda 430 }
MarcoF89 15:742683a8efda 431 } 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)));
MarcoF89 15:742683a8efda 432 (*z_off) = z_off1 + z_off2;
MarcoF89 15:742683a8efda 433 (*y_off) = y_off1 + y_off2;
MarcoF89 15:742683a8efda 434 (*x_off) = x_off1 + x_off2;
MarcoF89 15:742683a8efda 435 }
MarcoF89 15:742683a8efda 436
MarcoF89 15:742683a8efda 437 /****************/
MarcoF89 15:742683a8efda 438 /*Drift*****Gyro*/
MarcoF89 15:742683a8efda 439 /****************/
MarcoF89 15:742683a8efda 440 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)
MarcoF89 15:742683a8efda 441 {
MarcoF89 15:742683a8efda 442 timer->stop();
MarcoF89 15:742683a8efda 443 timer2->stop();
MarcoF89 15:742683a8efda 444 timer->reset();
MarcoF89 15:742683a8efda 445 timer2->reset();
MarcoF89 15:742683a8efda 446 timer->start();
MarcoF89 15:742683a8efda 447 timer2->start();
MarcoF89 15:742683a8efda 448 while(timer2->read_ms() <= 60000)
MarcoF89 15:742683a8efda 449 {
MarcoF89 15:742683a8efda 450 uint32_t zeit = timer->read_us();
MarcoF89 15:742683a8efda 451 timer->reset();
MarcoF89 15:742683a8efda 452 (*gain_g) = (*gain_g) + ((aktuell_gyro_z() - (*z_off)) * zeit * 0.000001 * 1/16.4);
MarcoF89 15:742683a8efda 453 (*pitch_g) = (*pitch_g) + ((aktuell_gyro_y() - (*y_off)) * zeit * 0.000001 * 1/16.4);
MarcoF89 15:742683a8efda 454 (*roll_g) = (*roll_g) + ((aktuell_gyro_x() - (*x_off)) * zeit * 0.000001 * 1/16.4);
MarcoF89 15:742683a8efda 455
MarcoF89 15:742683a8efda 456 }
MarcoF89 15:742683a8efda 457 (*drift_z) = (*gain_g)/30000; //Drift alle 4ms
MarcoF89 15:742683a8efda 458 (*drift_y) = (*pitch_g)/30000; //Drift alle 4ms
MarcoF89 15:742683a8efda 459 (*drift_x) = (*roll_g)/30000; //Drift alle 4ms
MarcoF89 15:742683a8efda 460 }
MarcoF89 25:a8a3cbc57c61 461
MarcoF89 25:a8a3cbc57c61 462
MarcoF89 25:a8a3cbc57c61 463 void Motorsteurung(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster2, DigitalIn *taster3, DigitalIn *taster4, int *n1, int *n2, int *n3, int *n4)
MarcoF89 25:a8a3cbc57c61 464 {
MarcoF89 25:a8a3cbc57c61 465 static bool flanke1, hilfe1 = 0, flanke2, hilfe2 = 0, flanke3, hilfe3 = 0, flanke4, hilfe4 = 0;
MarcoF89 25:a8a3cbc57c61 466
MarcoF89 25:a8a3cbc57c61 467 flanke2 = *taster2;
MarcoF89 25:a8a3cbc57c61 468 if ((flanke2 != 0) && (hilfe2 == 0))
MarcoF89 25:a8a3cbc57c61 469 {
chriselsholz 26:6c27486248fe 470 (*n1)+=50;
chriselsholz 26:6c27486248fe 471 (*n2)+=50;
chriselsholz 26:6c27486248fe 472 (*n3)+=50;
chriselsholz 26:6c27486248fe 473 (*n4)+=50;
MarcoF89 25:a8a3cbc57c61 474 }
MarcoF89 25:a8a3cbc57c61 475 hilfe2=flanke2;
chriselsholz 26:6c27486248fe 476 flanke3 = *taster3;
MarcoF89 25:a8a3cbc57c61 477 if ((flanke3 != 0) && (hilfe3 == 0))
MarcoF89 25:a8a3cbc57c61 478 {
MarcoF89 25:a8a3cbc57c61 479 (*n1)-=50;
MarcoF89 25:a8a3cbc57c61 480 (*n2)-=50;
MarcoF89 25:a8a3cbc57c61 481 (*n3)-=50;
MarcoF89 25:a8a3cbc57c61 482 (*n4)-=50;
MarcoF89 25:a8a3cbc57c61 483 }
MarcoF89 25:a8a3cbc57c61 484 hilfe3=flanke3;
chriselsholz 26:6c27486248fe 485 flanke4 = *taster4;
MarcoF89 25:a8a3cbc57c61 486 if ((flanke4 != 0) && (hilfe4 == 0))
MarcoF89 25:a8a3cbc57c61 487 {
MarcoF89 25:a8a3cbc57c61 488 (*n1)=(*n2)=(*n3)=(*n4)=650;
MarcoF89 25:a8a3cbc57c61 489 }
MarcoF89 25:a8a3cbc57c61 490 hilfe4=flanke4;
MarcoF89 25:a8a3cbc57c61 491 Motor1->pulsewidth_us(*n1);
MarcoF89 25:a8a3cbc57c61 492 Motor2->pulsewidth_us(*n2);
MarcoF89 25:a8a3cbc57c61 493 Motor3->pulsewidth_us(*n3);
MarcoF89 25:a8a3cbc57c61 494 Motor4->pulsewidth_us(*n4);
MarcoF89 25:a8a3cbc57c61 495 }