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@13:5f0a2103c707, 2017-08-17 (annotated)

Committer:
MarcoF89
Date:
Thu Aug 17 12:32:10 2017 +0000
Revision:
13:5f0a2103c707
Parent:
12:4a4dad7a3432
Child:
14:a75b20f9cc24
Funktionen erstellt

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 6:27a09e8bebfb 5 int16_t high, low;
MarcoF89 4:3eaf38e4809f 6
MarcoF89 4:3eaf38e4809f 7 extern Serial pc(SERIAL_TX, SERIAL_RX);
MarcoF89 4:3eaf38e4809f 8 extern SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk
MarcoF89 4:3eaf38e4809f 9 extern DigitalOut ncs(PE_4); //ssel
MarcoF89 4:3eaf38e4809f 10
MarcoF89 7:a54c97795013 11 extern AnalogIn potis_1 (PF_3);
MarcoF89 7:a54c97795013 12 extern AnalogIn potis_2 (PF_10);
MarcoF89 7:a54c97795013 13 extern AnalogIn potis_3 (PF_4);
MarcoF89 7:a54c97795013 14 extern AnalogIn potis_4 (PF_5);
MarcoF89 7:a54c97795013 15
MarcoF89 12:4a4dad7a3432 16 extern PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot
MarcoF89 12:4a4dad7a3432 17 extern PwmOut Motor2 (PC_9); // Weiß orange
MarcoF89 12:4a4dad7a3432 18 extern PwmOut Motor3 (PC_6); // Grau weiß
MarcoF89 12:4a4dad7a3432 19 extern PwmOut Motor4 (PB_9); // Blau braun
MarcoF89 12:4a4dad7a3432 20 // Gelb und Orange Vcc +5V
MarcoF89 12:4a4dad7a3432 21 // Gnd Rot
MarcoF89 12:4a4dad7a3432 22
MarcoF89 12:4a4dad7a3432 23
MarcoF89 7:a54c97795013 24
MarcoF89 12:4a4dad7a3432 25
MarcoF89 12:4a4dad7a3432 26 static int n1, n2, n3, n4;
MarcoF89 7:a54c97795013 27
chriselsholz 5:584acd257531 28 /**********************/
chriselsholz 5:584acd257531 29 /*Initialisieren**Gyro*/
chriselsholz 5:584acd257531 30 /**********************/
MarcoF89 12:4a4dad7a3432 31 void initialisierung_gyro()
chriselsholz 5:584acd257531 32 {
chriselsholz 5:584acd257531 33 spi.format(8,0);
chriselsholz 5:584acd257531 34 spi.frequency(1000000);
chriselsholz 5:584acd257531 35
chriselsholz 5:584acd257531 36 ncs = 0;
chriselsholz 5:584acd257531 37 spi.write(0x6B); // Register 107
chriselsholz 5:584acd257531 38 spi.write(0x80); //Reset // Standby off
chriselsholz 5:584acd257531 39 ncs = 1;
chriselsholz 5:584acd257531 40 wait_ms(1000);
chriselsholz 5:584acd257531 41
chriselsholz 5:584acd257531 42
chriselsholz 5:584acd257531 43 ncs=0;
chriselsholz 5:584acd257531 44 spi.write(0x1A); //CONFIG write // DLPF_CFG // Register 26
chriselsholz 5:584acd257531 45 spi.write(0x06); //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
chriselsholz 5:584acd257531 46 ncs = 1;
chriselsholz 5:584acd257531 47 wait_ms(1);
chriselsholz 5:584acd257531 48
chriselsholz 5:584acd257531 49 ncs=0;
chriselsholz 5:584acd257531 50 spi.write(0x1B); //Gyro_CONFIG write
chriselsholz 5:584acd257531 51 spi.write(0x18); //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
chriselsholz 5:584acd257531 52 ncs = 1;
chriselsholz 5:584acd257531 53 wait_ms(1);
chriselsholz 5:584acd257531 54
chriselsholz 5:584acd257531 55 ncs = 0;
chriselsholz 5:584acd257531 56 spi.write(0x17); // Register 23
MarcoF89 6:27a09e8bebfb 57 spi.write(0x00); // Offset High Byte Z
chriselsholz 5:584acd257531 58 ncs = 1;
chriselsholz 5:584acd257531 59 wait_ms(1);
chriselsholz 5:584acd257531 60
chriselsholz 5:584acd257531 61 ncs = 0;
chriselsholz 5:584acd257531 62 spi.write(0x18); // Register 24
MarcoF89 6:27a09e8bebfb 63 spi.write(0x17); // Offset Low Byte Z
chriselsholz 5:584acd257531 64 ncs = 1;
chriselsholz 5:584acd257531 65 wait_ms(1000);
chriselsholz 5:584acd257531 66 }
chriselsholz 5:584acd257531 67
chriselsholz 5:584acd257531 68 /**********************/
chriselsholz 5:584acd257531 69 /*Initialisieren Acc */
chriselsholz 5:584acd257531 70 /**********************/
chriselsholz 5:584acd257531 71
chriselsholz 5:584acd257531 72 int16_t initialisierung_acc ()
chriselsholz 5:584acd257531 73 {
chriselsholz 5:584acd257531 74 int i,faktor = 0x00;
chriselsholz 5:584acd257531 75 for(i=0;i<=2;i++)
chriselsholz 5:584acd257531 76 {
chriselsholz 5:584acd257531 77 ncs=0;
chriselsholz 5:584acd257531 78 spi.write(0x1c); //ACC_CONFIG write
chriselsholz 5:584acd257531 79 spi.write(faktor); //Skalierung 00=2g;08=4g;10=8g;18=16g
chriselsholz 5:584acd257531 80 ncs=1; //Teilung 16384;8192;4096;2048
chriselsholz 5:584acd257531 81 wait_us(0.1);
chriselsholz 5:584acd257531 82
chriselsholz 5:584acd257531 83 ncs=0;
chriselsholz 5:584acd257531 84 spi.write(0x1d); //ACC_CONFIG_2 08=460Hz;09=184Hz;0a=92Hz
MarcoF89 6:27a09e8bebfb 85 spi.write(0x0e); //TP-Filter 0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz
chriselsholz 5:584acd257531 86 ncs=1;
chriselsholz 5:584acd257531 87 wait_us(0.1);
chriselsholz 5:584acd257531 88 }
chriselsholz 5:584acd257531 89 switch (faktor)
chriselsholz 5:584acd257531 90 {
chriselsholz 5:584acd257531 91 case 0x00: return 16384; break;
chriselsholz 5:584acd257531 92 case 0x08: return 8192; break;
chriselsholz 5:584acd257531 93 case 0x10: return 4096; break;
chriselsholz 5:584acd257531 94 case 0x18: return 2048; break;
chriselsholz 5:584acd257531 95 }
chriselsholz 5:584acd257531 96 }
chriselsholz 5:584acd257531 97
chriselsholz 5:584acd257531 98
chriselsholz 5:584acd257531 99 /***************/
chriselsholz 5:584acd257531 100 /*Messen Gyro Z*/
chriselsholz 5:584acd257531 101 /***************/
MarcoF89 4:3eaf38e4809f 102
MarcoF89 6:27a09e8bebfb 103 int16_t aktuell_gyro_z()
MarcoF89 6:27a09e8bebfb 104 {
MarcoF89 4:3eaf38e4809f 105 ncs = 0;
MarcoF89 4:3eaf38e4809f 106 spi.write(0xc7); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 107 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 108 ncs = 1;
MarcoF89 6:27a09e8bebfb 109 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 110
MarcoF89 6:27a09e8bebfb 111 ncs = 0;
MarcoF89 6:27a09e8bebfb 112 spi.write(0xc8); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 113 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 114 ncs = 1;
MarcoF89 6:27a09e8bebfb 115 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 116 return (low | high << 8);
MarcoF89 6:27a09e8bebfb 117 }
MarcoF89 6:27a09e8bebfb 118
MarcoF89 6:27a09e8bebfb 119 /***************/
MarcoF89 6:27a09e8bebfb 120 /*Messen Gyro X*/
MarcoF89 6:27a09e8bebfb 121 /***************/
MarcoF89 6:27a09e8bebfb 122
MarcoF89 6:27a09e8bebfb 123 int16_t aktuell_gyro_x()
MarcoF89 6:27a09e8bebfb 124 {
MarcoF89 6:27a09e8bebfb 125 ncs = 0;
MarcoF89 6:27a09e8bebfb 126 spi.write(0xc3); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 127 high = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 128 ncs = 1;
MarcoF89 4:3eaf38e4809f 129 wait_us(1);
MarcoF89 4:3eaf38e4809f 130
MarcoF89 4:3eaf38e4809f 131 ncs = 0;
MarcoF89 6:27a09e8bebfb 132 spi.write(0xc4); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 133 low = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 134 ncs = 1;
MarcoF89 6:27a09e8bebfb 135 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 136 return low | high << 8;;
MarcoF89 6:27a09e8bebfb 137 }
MarcoF89 6:27a09e8bebfb 138
MarcoF89 6:27a09e8bebfb 139 /***************/
MarcoF89 6:27a09e8bebfb 140 /*Messen Gyro Y*/
MarcoF89 6:27a09e8bebfb 141 /***************/
MarcoF89 6:27a09e8bebfb 142
MarcoF89 6:27a09e8bebfb 143 int16_t aktuell_gyro_y()
MarcoF89 6:27a09e8bebfb 144 {
MarcoF89 6:27a09e8bebfb 145 ncs = 0;
MarcoF89 6:27a09e8bebfb 146 spi.write(0xc5); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 147 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 148 ncs = 1;
MarcoF89 6:27a09e8bebfb 149 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 150
MarcoF89 6:27a09e8bebfb 151 ncs = 0;
MarcoF89 6:27a09e8bebfb 152 spi.write(0xc6); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 153 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 154 ncs = 1;
MarcoF89 6:27a09e8bebfb 155 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 156 return low | high << 8;
MarcoF89 4:3eaf38e4809f 157 }
MarcoF89 4:3eaf38e4809f 158
chriselsholz 5:584acd257531 159 /************/
chriselsholz 5:584acd257531 160 /*Messen Acc*/
chriselsholz 5:584acd257531 161 /************/
MarcoF89 4:3eaf38e4809f 162
chriselsholz 5:584acd257531 163 int16_t aktuell_acc_x ()
chriselsholz 5:584acd257531 164 {
MarcoF89 4:3eaf38e4809f 165 ncs=0;
chriselsholz 5:584acd257531 166 spi.write(0xbb);
MarcoF89 6:27a09e8bebfb 167 high = spi.write(0x0);
chriselsholz 5:584acd257531 168 ncs=1;
chriselsholz 5:584acd257531 169 wait_us(0.1);
chriselsholz 5:584acd257531 170
chriselsholz 5:584acd257531 171 ncs=0;
chriselsholz 5:584acd257531 172 spi.write(0xbc);
MarcoF89 6:27a09e8bebfb 173 low = spi.write(0x0);
chriselsholz 5:584acd257531 174 ncs=1;
MarcoF89 6:27a09e8bebfb 175 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 176 return low | high<<8;
chriselsholz 5:584acd257531 177 }
MarcoF89 4:3eaf38e4809f 178
chriselsholz 5:584acd257531 179 int16_t aktuell_acc_y ()
chriselsholz 5:584acd257531 180 {
chriselsholz 5:584acd257531 181 ncs=0;
chriselsholz 5:584acd257531 182 spi.write(0xbd);
MarcoF89 6:27a09e8bebfb 183 high = spi.write(0x0);
chriselsholz 5:584acd257531 184 ncs=1;
chriselsholz 5:584acd257531 185 wait_us(0.1);
chriselsholz 5:584acd257531 186
chriselsholz 5:584acd257531 187 ncs=0;
chriselsholz 5:584acd257531 188 spi.write(0xbe);
MarcoF89 6:27a09e8bebfb 189 low = spi.write(0x0);
chriselsholz 5:584acd257531 190 ncs=1;
MarcoF89 6:27a09e8bebfb 191 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 192 return low | high<<8;
chriselsholz 5:584acd257531 193 }
MarcoF89 4:3eaf38e4809f 194
chriselsholz 5:584acd257531 195 int16_t aktuell_acc_z ()
chriselsholz 5:584acd257531 196 {
chriselsholz 5:584acd257531 197 ncs=0;
chriselsholz 5:584acd257531 198 spi.write(0xbf);
MarcoF89 6:27a09e8bebfb 199 high = spi.write(0x0);
chriselsholz 5:584acd257531 200 ncs=1;
chriselsholz 5:584acd257531 201 wait_us(0.1);
chriselsholz 5:584acd257531 202
chriselsholz 5:584acd257531 203 ncs=0;
chriselsholz 5:584acd257531 204 spi.write(0xc0);
MarcoF89 6:27a09e8bebfb 205 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 206 ncs=1;
MarcoF89 6:27a09e8bebfb 207 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 208 return low | high<<8;
chriselsholz 5:584acd257531 209 }
MarcoF89 12:4a4dad7a3432 210
MarcoF89 13:5f0a2103c707 211 /**********/
MarcoF89 13:5f0a2103c707 212 /*Anlernen*/
MarcoF89 13:5f0a2103c707 213 /**********/
MarcoF89 12:4a4dad7a3432 214
MarcoF89 13:5f0a2103c707 215 void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 216 {
MarcoF89 13:5f0a2103c707 217 int n1 = n2 = n3 = n4 = 1900;
MarcoF89 13:5f0a2103c707 218 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 219 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 220 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 221 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 222 pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r");
MarcoF89 13:5f0a2103c707 223 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 224 while (!*taster4)
MarcoF89 13:5f0a2103c707 225 {
MarcoF89 13:5f0a2103c707 226 if (*taster1)
MarcoF89 13:5f0a2103c707 227 {
MarcoF89 13:5f0a2103c707 228 n1 = n2 =n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 229 }
MarcoF89 13:5f0a2103c707 230 if (*taster2)
MarcoF89 13:5f0a2103c707 231 {
MarcoF89 13:5f0a2103c707 232 n1 = n2 = n3 = n4 =1900;
MarcoF89 13:5f0a2103c707 233 }
MarcoF89 13:5f0a2103c707 234 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 235 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 236 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 237 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 238 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 239 }
MarcoF89 13:5f0a2103c707 240 }
MarcoF89 13:5f0a2103c707 241
MarcoF89 13:5f0a2103c707 242 /**********/
MarcoF89 13:5f0a2103c707 243 /*Rauschen*/
MarcoF89 13:5f0a2103c707 244 /**********/
MarcoF89 13:5f0a2103c707 245 void viberationen(AnalogOut *rauschen, PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 246 {
MarcoF89 13:5f0a2103c707 247 (*rauschen) = 1;
MarcoF89 13:5f0a2103c707 248 int n1 = n2 = n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 249 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 250 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 251 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 252 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 253 wait_ms(10000);
MarcoF89 13:5f0a2103c707 254 while(!*taster4)
MarcoF89 13:5f0a2103c707 255 {
MarcoF89 13:5f0a2103c707 256 for(int i = 1; i <= 1000; i++)
MarcoF89 13:5f0a2103c707 257 {
MarcoF89 13:5f0a2103c707 258 float k = aktuell_acc_x()*aktuell_acc_x();
MarcoF89 13:5f0a2103c707 259 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 260 pc.printf("Winkel:%2.5f\n\r",k);
MarcoF89 13:5f0a2103c707 261 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 262 wait_ms(10);
MarcoF89 13:5f0a2103c707 263 }
MarcoF89 13:5f0a2103c707 264 n1 = n2 = n3 = n4 = 1400;
MarcoF89 13:5f0a2103c707 265 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 266 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 267 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 268 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 269 for(int i = 1; i <= 3000; i++)
MarcoF89 13:5f0a2103c707 270 {
MarcoF89 13:5f0a2103c707 271 float k = aktuell_acc_x() * aktuell_acc_x();
MarcoF89 13:5f0a2103c707 272 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 273 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 274 wait_ms(10);
MarcoF89 13:5f0a2103c707 275 }
MarcoF89 13:5f0a2103c707 276 (*rauschen) = 0;
MarcoF89 13:5f0a2103c707 277 wait_ms(100000);
MarcoF89 13:5f0a2103c707 278 }
MarcoF89 13:5f0a2103c707 279 }