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@14:a75b20f9cc24, 2017-08-17 (annotated)

Committer:
chriselsholz
Date:
Thu Aug 17 12:55:14 2017 +0000
Revision:
14:a75b20f9cc24
Parent:
13:5f0a2103c707
Child:
15:742683a8efda
1. im Ordner "display" die TextLCD-Library eingef?gt; 2. in "messen.cpp" Funktionen f?r die Winkel "Pitch" u "Roll" eingef?gt

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"
chriselsholz 14:a75b20f9cc24 3 #define PI 3.14159
MarcoF89 4:3eaf38e4809f 4
MarcoF89 12:4a4dad7a3432 5
MarcoF89 6:27a09e8bebfb 6 int16_t high, low;
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 12:4a4dad7a3432 24
MarcoF89 7:a54c97795013 25
MarcoF89 12:4a4dad7a3432 26
MarcoF89 12:4a4dad7a3432 27 static int n1, n2, n3, n4;
MarcoF89 7:a54c97795013 28
chriselsholz 5:584acd257531 29 /**********************/
chriselsholz 5:584acd257531 30 /*Initialisieren**Gyro*/
chriselsholz 5:584acd257531 31 /**********************/
MarcoF89 12:4a4dad7a3432 32 void initialisierung_gyro()
chriselsholz 5:584acd257531 33 {
chriselsholz 5:584acd257531 34 spi.format(8,0);
chriselsholz 5:584acd257531 35 spi.frequency(1000000);
chriselsholz 5:584acd257531 36
chriselsholz 5:584acd257531 37 ncs = 0;
chriselsholz 5:584acd257531 38 spi.write(0x6B); // Register 107
chriselsholz 5:584acd257531 39 spi.write(0x80); //Reset // Standby off
chriselsholz 5:584acd257531 40 ncs = 1;
chriselsholz 5:584acd257531 41 wait_ms(1000);
chriselsholz 5:584acd257531 42
chriselsholz 5:584acd257531 43
chriselsholz 5:584acd257531 44 ncs=0;
chriselsholz 5:584acd257531 45 spi.write(0x1A); //CONFIG write // DLPF_CFG // Register 26
chriselsholz 5:584acd257531 46 spi.write(0x06); //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
chriselsholz 5:584acd257531 47 ncs = 1;
chriselsholz 5:584acd257531 48 wait_ms(1);
chriselsholz 5:584acd257531 49
chriselsholz 5:584acd257531 50 ncs=0;
chriselsholz 5:584acd257531 51 spi.write(0x1B); //Gyro_CONFIG write
chriselsholz 5:584acd257531 52 spi.write(0x18); //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
chriselsholz 5:584acd257531 53 ncs = 1;
chriselsholz 5:584acd257531 54 wait_ms(1);
chriselsholz 5:584acd257531 55
chriselsholz 5:584acd257531 56 ncs = 0;
chriselsholz 5:584acd257531 57 spi.write(0x17); // Register 23
MarcoF89 6:27a09e8bebfb 58 spi.write(0x00); // Offset High Byte Z
chriselsholz 5:584acd257531 59 ncs = 1;
chriselsholz 5:584acd257531 60 wait_ms(1);
chriselsholz 5:584acd257531 61
chriselsholz 5:584acd257531 62 ncs = 0;
chriselsholz 5:584acd257531 63 spi.write(0x18); // Register 24
MarcoF89 6:27a09e8bebfb 64 spi.write(0x17); // Offset Low Byte Z
chriselsholz 5:584acd257531 65 ncs = 1;
chriselsholz 5:584acd257531 66 wait_ms(1000);
chriselsholz 5:584acd257531 67 }
chriselsholz 5:584acd257531 68
chriselsholz 5:584acd257531 69 /**********************/
chriselsholz 5:584acd257531 70 /*Initialisieren Acc */
chriselsholz 5:584acd257531 71 /**********************/
chriselsholz 5:584acd257531 72
chriselsholz 5:584acd257531 73 int16_t initialisierung_acc ()
chriselsholz 5:584acd257531 74 {
chriselsholz 5:584acd257531 75 int i,faktor = 0x00;
chriselsholz 5:584acd257531 76 for(i=0;i<=2;i++)
chriselsholz 5:584acd257531 77 {
chriselsholz 5:584acd257531 78 ncs=0;
chriselsholz 5:584acd257531 79 spi.write(0x1c); //ACC_CONFIG write
chriselsholz 5:584acd257531 80 spi.write(faktor); //Skalierung 00=2g;08=4g;10=8g;18=16g
chriselsholz 5:584acd257531 81 ncs=1; //Teilung 16384;8192;4096;2048
chriselsholz 5:584acd257531 82 wait_us(0.1);
chriselsholz 5:584acd257531 83
chriselsholz 5:584acd257531 84 ncs=0;
chriselsholz 5:584acd257531 85 spi.write(0x1d); //ACC_CONFIG_2 08=460Hz;09=184Hz;0a=92Hz
MarcoF89 6:27a09e8bebfb 86 spi.write(0x0e); //TP-Filter 0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz
chriselsholz 5:584acd257531 87 ncs=1;
chriselsholz 5:584acd257531 88 wait_us(0.1);
chriselsholz 5:584acd257531 89 }
chriselsholz 5:584acd257531 90 switch (faktor)
chriselsholz 5:584acd257531 91 {
chriselsholz 14:a75b20f9cc24 92 case 0x00: faktor = 16384; break;
chriselsholz 14:a75b20f9cc24 93 case 0x08: faktor = 8192; break;
chriselsholz 14:a75b20f9cc24 94 case 0x10: faktor = 4096; break;
chriselsholz 14:a75b20f9cc24 95 case 0x18: faktor = 2048; break;
chriselsholz 5:584acd257531 96 }
chriselsholz 14:a75b20f9cc24 97 return faktor;
chriselsholz 5:584acd257531 98 }
chriselsholz 5:584acd257531 99
chriselsholz 5:584acd257531 100
chriselsholz 5:584acd257531 101 /***************/
chriselsholz 5:584acd257531 102 /*Messen Gyro Z*/
chriselsholz 5:584acd257531 103 /***************/
MarcoF89 4:3eaf38e4809f 104
MarcoF89 6:27a09e8bebfb 105 int16_t aktuell_gyro_z()
MarcoF89 6:27a09e8bebfb 106 {
MarcoF89 4:3eaf38e4809f 107 ncs = 0;
MarcoF89 4:3eaf38e4809f 108 spi.write(0xc7); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 109 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 110 ncs = 1;
MarcoF89 6:27a09e8bebfb 111 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 112
MarcoF89 6:27a09e8bebfb 113 ncs = 0;
MarcoF89 6:27a09e8bebfb 114 spi.write(0xc8); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 115 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 116 ncs = 1;
MarcoF89 6:27a09e8bebfb 117 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 118 return (low | high << 8);
MarcoF89 6:27a09e8bebfb 119 }
MarcoF89 6:27a09e8bebfb 120
MarcoF89 6:27a09e8bebfb 121 /***************/
MarcoF89 6:27a09e8bebfb 122 /*Messen Gyro X*/
MarcoF89 6:27a09e8bebfb 123 /***************/
MarcoF89 6:27a09e8bebfb 124
MarcoF89 6:27a09e8bebfb 125 int16_t aktuell_gyro_x()
MarcoF89 6:27a09e8bebfb 126 {
MarcoF89 6:27a09e8bebfb 127 ncs = 0;
MarcoF89 6:27a09e8bebfb 128 spi.write(0xc3); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 129 high = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 130 ncs = 1;
MarcoF89 4:3eaf38e4809f 131 wait_us(1);
MarcoF89 4:3eaf38e4809f 132
MarcoF89 4:3eaf38e4809f 133 ncs = 0;
MarcoF89 6:27a09e8bebfb 134 spi.write(0xc4); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 135 low = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 136 ncs = 1;
MarcoF89 6:27a09e8bebfb 137 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 138 return low | high << 8;;
MarcoF89 6:27a09e8bebfb 139 }
MarcoF89 6:27a09e8bebfb 140
MarcoF89 6:27a09e8bebfb 141 /***************/
MarcoF89 6:27a09e8bebfb 142 /*Messen Gyro Y*/
MarcoF89 6:27a09e8bebfb 143 /***************/
MarcoF89 6:27a09e8bebfb 144
MarcoF89 6:27a09e8bebfb 145 int16_t aktuell_gyro_y()
MarcoF89 6:27a09e8bebfb 146 {
MarcoF89 6:27a09e8bebfb 147 ncs = 0;
MarcoF89 6:27a09e8bebfb 148 spi.write(0xc5); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 149 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 150 ncs = 1;
MarcoF89 6:27a09e8bebfb 151 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 152
MarcoF89 6:27a09e8bebfb 153 ncs = 0;
MarcoF89 6:27a09e8bebfb 154 spi.write(0xc6); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 155 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 156 ncs = 1;
MarcoF89 6:27a09e8bebfb 157 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 158 return low | high << 8;
MarcoF89 4:3eaf38e4809f 159 }
MarcoF89 4:3eaf38e4809f 160
chriselsholz 5:584acd257531 161 /************/
chriselsholz 5:584acd257531 162 /*Messen Acc*/
chriselsholz 5:584acd257531 163 /************/
MarcoF89 4:3eaf38e4809f 164
chriselsholz 5:584acd257531 165 int16_t aktuell_acc_x ()
chriselsholz 5:584acd257531 166 {
MarcoF89 4:3eaf38e4809f 167 ncs=0;
chriselsholz 5:584acd257531 168 spi.write(0xbb);
MarcoF89 6:27a09e8bebfb 169 high = spi.write(0x0);
chriselsholz 5:584acd257531 170 ncs=1;
chriselsholz 5:584acd257531 171 wait_us(0.1);
chriselsholz 5:584acd257531 172
chriselsholz 5:584acd257531 173 ncs=0;
chriselsholz 5:584acd257531 174 spi.write(0xbc);
MarcoF89 6:27a09e8bebfb 175 low = spi.write(0x0);
chriselsholz 5:584acd257531 176 ncs=1;
MarcoF89 6:27a09e8bebfb 177 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 178 return low | high<<8;
chriselsholz 5:584acd257531 179 }
MarcoF89 4:3eaf38e4809f 180
chriselsholz 14:a75b20f9cc24 181 float read_x_acc ()
chriselsholz 14:a75b20f9cc24 182 {
chriselsholz 14:a75b20f9cc24 183 float xd = aktuell_acc_x();
chriselsholz 14:a75b20f9cc24 184 xd/=16384;
chriselsholz 14:a75b20f9cc24 185 if(xd<-1.0){return -1.0;}
chriselsholz 14:a75b20f9cc24 186 if(xd>1.0){return 1.0;}
chriselsholz 14:a75b20f9cc24 187 else return xd;
chriselsholz 14:a75b20f9cc24 188 }
chriselsholz 14:a75b20f9cc24 189
chriselsholz 5:584acd257531 190 int16_t aktuell_acc_y ()
chriselsholz 5:584acd257531 191 {
chriselsholz 5:584acd257531 192 ncs=0;
chriselsholz 5:584acd257531 193 spi.write(0xbd);
MarcoF89 6:27a09e8bebfb 194 high = spi.write(0x0);
chriselsholz 5:584acd257531 195 ncs=1;
chriselsholz 5:584acd257531 196 wait_us(0.1);
chriselsholz 5:584acd257531 197
chriselsholz 5:584acd257531 198 ncs=0;
chriselsholz 5:584acd257531 199 spi.write(0xbe);
MarcoF89 6:27a09e8bebfb 200 low = spi.write(0x0);
chriselsholz 5:584acd257531 201 ncs=1;
MarcoF89 6:27a09e8bebfb 202 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 203 return low | high<<8;
chriselsholz 5:584acd257531 204 }
MarcoF89 4:3eaf38e4809f 205
chriselsholz 14:a75b20f9cc24 206 float read_y_acc ()
chriselsholz 14:a75b20f9cc24 207 {
chriselsholz 14:a75b20f9cc24 208 float yd = aktuell_acc_y();
chriselsholz 14:a75b20f9cc24 209 xd/=16384;
chriselsholz 14:a75b20f9cc24 210 if(yd<-1.0){return -1.0;}
chriselsholz 14:a75b20f9cc24 211 if(yd>1.0){return 1.0;}
chriselsholz 14:a75b20f9cc24 212 else return yd;
chriselsholz 14:a75b20f9cc24 213 }
chriselsholz 14:a75b20f9cc24 214
chriselsholz 5:584acd257531 215 int16_t aktuell_acc_z ()
chriselsholz 5:584acd257531 216 {
chriselsholz 5:584acd257531 217 ncs=0;
chriselsholz 5:584acd257531 218 spi.write(0xbf);
MarcoF89 6:27a09e8bebfb 219 high = spi.write(0x0);
chriselsholz 5:584acd257531 220 ncs=1;
chriselsholz 5:584acd257531 221 wait_us(0.1);
chriselsholz 5:584acd257531 222
chriselsholz 5:584acd257531 223 ncs=0;
chriselsholz 5:584acd257531 224 spi.write(0xc0);
MarcoF89 6:27a09e8bebfb 225 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 226 ncs=1;
MarcoF89 6:27a09e8bebfb 227 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 228 return low | high<<8;
chriselsholz 5:584acd257531 229 }
MarcoF89 12:4a4dad7a3432 230
chriselsholz 14:a75b20f9cc24 231 float read_z_acc ()
chriselsholz 14:a75b20f9cc24 232 {
chriselsholz 14:a75b20f9cc24 233 float zd = aktuell_acc_z();
chriselsholz 14:a75b20f9cc24 234 zd/=16384;
chriselsholz 14:a75b20f9cc24 235 if(zd<-1.0){return -1.0;}
chriselsholz 14:a75b20f9cc24 236 if(zd>1.0){return 1.0;}
chriselsholz 14:a75b20f9cc24 237 else return zd;
chriselsholz 14:a75b20f9cc24 238 }
chriselsholz 14:a75b20f9cc24 239
chriselsholz 14:a75b20f9cc24 240 double pitch ()
chriselsholz 14:a75b20f9cc24 241 {
chriselsholz 14:a75b20f9cc24 242 double x,z;
chriselsholz 14:a75b20f9cc24 243 x=read_x_acc(); z=read_z_acc();
chriselsholz 14:a75b20f9cc24 244 double pitch = atan2(-x,z)*180/PI;
chriselsholz 14:a75b20f9cc24 245 return pitch;
chriselsholz 14:a75b20f9cc24 246 }
chriselsholz 14:a75b20f9cc24 247
chriselsholz 14:a75b20f9cc24 248 double roll ()
chriselsholz 14:a75b20f9cc24 249 {
chriselsholz 14:a75b20f9cc24 250 double x,y,z;
chriselsholz 14:a75b20f9cc24 251 y=read_y_acc(); x=pow(read_x_acc(),2); z=pow(read_z_acc(),2);
chriselsholz 14:a75b20f9cc24 252 double roll = atan2(y,sqrt(x+z))*180/PI;
chriselsholz 14:a75b20f9cc24 253 return roll;
chriselsholz 14:a75b20f9cc24 254 }
chriselsholz 14:a75b20f9cc24 255
chriselsholz 14:a75b20f9cc24 256
MarcoF89 13:5f0a2103c707 257 /**********/
MarcoF89 13:5f0a2103c707 258 /*Anlernen*/
MarcoF89 13:5f0a2103c707 259 /**********/
MarcoF89 12:4a4dad7a3432 260
MarcoF89 13:5f0a2103c707 261 void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 262 {
MarcoF89 13:5f0a2103c707 263 int n1 = n2 = n3 = n4 = 1900;
MarcoF89 13:5f0a2103c707 264 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 265 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 266 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 267 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 268 pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r");
MarcoF89 13:5f0a2103c707 269 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 270 while (!*taster4)
MarcoF89 13:5f0a2103c707 271 {
MarcoF89 13:5f0a2103c707 272 if (*taster1)
MarcoF89 13:5f0a2103c707 273 {
MarcoF89 13:5f0a2103c707 274 n1 = n2 =n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 275 }
MarcoF89 13:5f0a2103c707 276 if (*taster2)
MarcoF89 13:5f0a2103c707 277 {
MarcoF89 13:5f0a2103c707 278 n1 = n2 = n3 = n4 =1900;
MarcoF89 13:5f0a2103c707 279 }
MarcoF89 13:5f0a2103c707 280 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 281 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 282 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 283 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 284 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 13:5f0a2103c707 285 }
MarcoF89 13:5f0a2103c707 286 }
MarcoF89 13:5f0a2103c707 287
MarcoF89 13:5f0a2103c707 288 /**********/
MarcoF89 13:5f0a2103c707 289 /*Rauschen*/
MarcoF89 13:5f0a2103c707 290 /**********/
MarcoF89 13:5f0a2103c707 291 void viberationen(AnalogOut *rauschen, PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster4)
MarcoF89 13:5f0a2103c707 292 {
MarcoF89 13:5f0a2103c707 293 (*rauschen) = 1;
MarcoF89 13:5f0a2103c707 294 int n1 = n2 = n3 = n4 = 700;
MarcoF89 13:5f0a2103c707 295 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 296 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 297 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 298 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 299 wait_ms(10000);
MarcoF89 13:5f0a2103c707 300 while(!*taster4)
MarcoF89 13:5f0a2103c707 301 {
MarcoF89 13:5f0a2103c707 302 for(int i = 1; i <= 1000; i++)
MarcoF89 13:5f0a2103c707 303 {
MarcoF89 13:5f0a2103c707 304 float k = aktuell_acc_x()*aktuell_acc_x();
MarcoF89 13:5f0a2103c707 305 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 306 pc.printf("Winkel:%2.5f\n\r",k);
MarcoF89 13:5f0a2103c707 307 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 308 wait_ms(10);
MarcoF89 13:5f0a2103c707 309 }
MarcoF89 13:5f0a2103c707 310 n1 = n2 = n3 = n4 = 1400;
MarcoF89 13:5f0a2103c707 311 Motor1->pulsewidth_us(n1);
MarcoF89 13:5f0a2103c707 312 Motor2->pulsewidth_us(n2);
MarcoF89 13:5f0a2103c707 313 Motor3->pulsewidth_us(n3);
MarcoF89 13:5f0a2103c707 314 Motor4->pulsewidth_us(n4);
MarcoF89 13:5f0a2103c707 315 for(int i = 1; i <= 3000; i++)
MarcoF89 13:5f0a2103c707 316 {
MarcoF89 13:5f0a2103c707 317 float k = aktuell_acc_x() * aktuell_acc_x();
MarcoF89 13:5f0a2103c707 318 k = sqrt(k) * 0.0000438596491;
MarcoF89 13:5f0a2103c707 319 (*rauschen) = k;
MarcoF89 13:5f0a2103c707 320 wait_ms(10);
MarcoF89 13:5f0a2103c707 321 }
MarcoF89 13:5f0a2103c707 322 (*rauschen) = 0;
MarcoF89 13:5f0a2103c707 323 wait_ms(100000);
MarcoF89 13:5f0a2103c707 324 }
MarcoF89 13:5f0a2103c707 325 }