Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@5:c3caf8b83e6b, 2014-12-19 (annotated)

Committer:
Michu90
Date:
Fri Dec 19 13:33:21 2014 +0000
Revision:
5:c3caf8b83e6b
Parent:
4:a5b51a651db7
Child:
6:8cc6df266363
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:50d2b9c62765 1 #include "mbed.h"
Igor_W 3:1425359662e4 2 #include "FastPWM.h"
Michu90 5:c3caf8b83e6b 3 #include "kalman.h"
Michu90 5:c3caf8b83e6b 4 #include "Offsets.h"
Michu90 5:c3caf8b83e6b 5 #include "stdio.h"
Michu90 5:c3caf8b83e6b 6 #include "IMU.h"
Michu90 5:c3caf8b83e6b 7
Igor_W 2:9e6ed6302c78 8 #define PWM_period 2500
Michu90 5:c3caf8b83e6b 9 #define M_PI 3.141592
Michu90 5:c3caf8b83e6b 10 #define M_PI2 1.570796
Michu90 5:c3caf8b83e6b 11 #define dt 0.005
Michu90 5:c3caf8b83e6b 12
Michu90 5:c3caf8b83e6b 13
Michu90 5:c3caf8b83e6b 14 //kalman
Michu90 5:c3caf8b83e6b 15 // Structs for containing filter data
Michu90 5:c3caf8b83e6b 16 kalman_data pitch_data;
Michu90 5:c3caf8b83e6b 17 kalman_data roll_data;
Michu90 5:c3caf8b83e6b 18
Michu90 5:c3caf8b83e6b 19 kalman_data pitch_data2;
Michu90 5:c3caf8b83e6b 20 kalman_data roll_data2;
Michu90 5:c3caf8b83e6b 21
Michu90 5:c3caf8b83e6b 22 DigitalOut myled(PTA2);
Michu90 5:c3caf8b83e6b 23 DigitalOut myled2(PTA1);
Michu90 5:c3caf8b83e6b 24 Serial pc(USBTX, USBRX);
Michu90 5:c3caf8b83e6b 25 Serial bluetooth(D1, D0);
Michu90 5:c3caf8b83e6b 26 IMU imu(PTE25,PTE24);
Michu90 5:c3caf8b83e6b 27 Offsets off;
Michu90 5:c3caf8b83e6b 28
Michu90 5:c3caf8b83e6b 29 FastPWM M1(D10);
Michu90 5:c3caf8b83e6b 30 FastPWM M2(D11);
Michu90 5:c3caf8b83e6b 31 FastPWM M3(D12);
Michu90 5:c3caf8b83e6b 32 FastPWM M4(D13);
Igor_W 2:9e6ed6302c78 33
Igor_W 2:9e6ed6302c78 34
Michu90 5:c3caf8b83e6b 35 Ticker triger1; //przerwanie filtracji
Michu90 5:c3caf8b83e6b 36 //Ticker triger2; //przerwanie wysyłania danych
Igor_W 2:9e6ed6302c78 37
Michu90 5:c3caf8b83e6b 38 float d[9];
Michu90 5:c3caf8b83e6b 39 double D[9];
Michu90 5:c3caf8b83e6b 40 float o[3];
Michu90 5:c3caf8b83e6b 41 float O[3];
Michu90 5:c3caf8b83e6b 42 char buff[160];
Michu90 5:c3caf8b83e6b 43 float r,katx,katy;
Michu90 5:c3caf8b83e6b 44 float rbut,katxbut,katybut;
Michu90 5:c3caf8b83e6b 45 float pitch, roll;
Michu90 5:c3caf8b83e6b 46 float pitch2, roll2;
Michu90 5:c3caf8b83e6b 47 double i;
Michu90 5:c3caf8b83e6b 48 float offsetGyro[3];
Michu90 5:c3caf8b83e6b 49 char odczyt[20];
Michu90 4:a5b51a651db7 50 char znak;
Michu90 4:a5b51a651db7 51 char znak2;
Michu90 5:c3caf8b83e6b 52
Michu90 5:c3caf8b83e6b 53 double PWM1zad;
Michu90 5:c3caf8b83e6b 54 double PWM2zad;
Michu90 5:c3caf8b83e6b 55 double PWM3zad;
Michu90 5:c3caf8b83e6b 56 double PWM4zad;
Michu90 5:c3caf8b83e6b 57 double valPWM1;
Michu90 5:c3caf8b83e6b 58 double valPWM2;
Michu90 5:c3caf8b83e6b 59 double valPWM3;
Michu90 5:c3caf8b83e6b 60 double valPWM4;
Michu90 5:c3caf8b83e6b 61
Michu90 5:c3caf8b83e6b 62 void task1()
Michu90 5:c3caf8b83e6b 63 {
Michu90 5:c3caf8b83e6b 64 imu.readData(d);
Michu90 5:c3caf8b83e6b 65 imu.filterData(d, D);
Michu90 5:c3caf8b83e6b 66 off.offsetData(d,offsetGyro,o);
Michu90 5:c3caf8b83e6b 67 off.offsetData2(D,offsetGyro,O);
Michu90 5:c3caf8b83e6b 68
Michu90 5:c3caf8b83e6b 69 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 5:c3caf8b83e6b 70 katx = acos(d[4]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 71 katy = acos(d[3]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 72
Michu90 5:c3caf8b83e6b 73 rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
Michu90 5:c3caf8b83e6b 74 katxbut = acos(D[4]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 75 katybut = acos(D[3]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 76
Michu90 5:c3caf8b83e6b 77
Michu90 5:c3caf8b83e6b 78 //Filtr Kalmana
Michu90 5:c3caf8b83e6b 79 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 5:c3caf8b83e6b 80 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 5:c3caf8b83e6b 81 pitch = pitch_data.x1;
Michu90 5:c3caf8b83e6b 82 roll = roll_data.x1;
Michu90 5:c3caf8b83e6b 83
Michu90 5:c3caf8b83e6b 84 //Filtr Kalmana butterworth 2nd
Michu90 5:c3caf8b83e6b 85 kalman_innovate(&pitch_data2, katxbut, O[0]);
Michu90 5:c3caf8b83e6b 86 kalman_innovate(&roll_data2, -katybut, O[1]);
Michu90 5:c3caf8b83e6b 87 pitch2 = pitch_data2.x1;
Michu90 5:c3caf8b83e6b 88 roll2 = roll_data2.x1;
Michu90 5:c3caf8b83e6b 89
Michu90 5:c3caf8b83e6b 90
Michu90 5:c3caf8b83e6b 91 //sprintf(buff, "%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI);
Michu90 5:c3caf8b83e6b 92 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 93 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 94 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 95 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", offsetGyro[0]*180/M_PI, offsetGyro[1]*180/M_PI, offsetGyro[2]*180/M_PI, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 96 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 97 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 98 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 99
Michu90 5:c3caf8b83e6b 100 myled = !myled;
Michu90 5:c3caf8b83e6b 101 }
Michu90 5:c3caf8b83e6b 102
Michu90 5:c3caf8b83e6b 103
Michu90 5:c3caf8b83e6b 104 void task2()
Michu90 5:c3caf8b83e6b 105 {
Michu90 5:c3caf8b83e6b 106 /*
Michu90 5:c3caf8b83e6b 107 sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI, katx*180/M_PI, pitch*180/M_PI, katxbut*180/M_PI, pitch2*180/M_PI,(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 108 pc.printf(buff);
Michu90 5:c3caf8b83e6b 109 myled2 = !myled2;*/
Michu90 5:c3caf8b83e6b 110 }
Michu90 5:c3caf8b83e6b 111
Michu90 5:c3caf8b83e6b 112
Michu90 5:c3caf8b83e6b 113
Michu90 4:a5b51a651db7 114
mbed_official 0:50d2b9c62765 115 int main() {
Igor_W 2:9e6ed6302c78 116
Michu90 4:a5b51a651db7 117 pc.baud(115200);
Michu90 4:a5b51a651db7 118 bluetooth.baud(19200);
Michu90 5:c3caf8b83e6b 119 imu.init();
Michu90 5:c3caf8b83e6b 120 kalman_init(&pitch_data);
Michu90 5:c3caf8b83e6b 121 kalman_init(&roll_data);
Michu90 5:c3caf8b83e6b 122 kalman_init(&pitch_data2);
Michu90 5:c3caf8b83e6b 123 kalman_init(&roll_data2);
Igor_W 3:1425359662e4 124
Michu90 4:a5b51a651db7 125 sprintf(buff, "Hello: \n\r");
Michu90 4:a5b51a651db7 126 pc.printf(buff);
Michu90 4:a5b51a651db7 127
Michu90 5:c3caf8b83e6b 128 //off.setOffsets(offsetGyro, pc, imu);
Michu90 5:c3caf8b83e6b 129
Michu90 5:c3caf8b83e6b 130
Michu90 5:c3caf8b83e6b 131
Michu90 5:c3caf8b83e6b 132 triger1.attach(&task1, 0.005);
Michu90 5:c3caf8b83e6b 133 //triger2.attach(&task2, 0.005);
Michu90 5:c3caf8b83e6b 134 i=1000;
Michu90 5:c3caf8b83e6b 135
Igor_W 3:1425359662e4 136
Michu90 5:c3caf8b83e6b 137 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 138 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 139 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 140 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 141
Michu90 5:c3caf8b83e6b 142 M1.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 143 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 144 M2.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 145 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 146 M3.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 147 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 148 M4.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 149 M4.pulsewidth_us(PWM4zad);
Michu90 4:a5b51a651db7 150
Igor_W 2:9e6ed6302c78 151 while(1) {
Michu90 4:a5b51a651db7 152
Michu90 5:c3caf8b83e6b 153 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI, katx*180/M_PI, pitch*180/M_PI, katxbut*180/M_PI, pitch2*180/M_PI,(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
Michu90 5:c3caf8b83e6b 154 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 155 //myled2 = !myled2;
Michu90 5:c3caf8b83e6b 156
Michu90 4:a5b51a651db7 157
Michu90 4:a5b51a651db7 158 if(pc.readable()){
Michu90 4:a5b51a651db7 159 znak=pc.getc();
Michu90 4:a5b51a651db7 160 switch (znak){
Michu90 5:c3caf8b83e6b 161
Michu90 4:a5b51a651db7 162 case 'p':
Michu90 4:a5b51a651db7 163 sprintf(buff, "odczytany znak: %c\n\r",znak);
Michu90 4:a5b51a651db7 164 pc.printf(buff);
Michu90 4:a5b51a651db7 165
Michu90 4:a5b51a651db7 166 i+=10;
Michu90 4:a5b51a651db7 167 break;
Michu90 4:a5b51a651db7 168
Michu90 4:a5b51a651db7 169 case 'm':
Michu90 4:a5b51a651db7 170 i-=10;
Michu90 4:a5b51a651db7 171 break;
Michu90 4:a5b51a651db7 172
Michu90 4:a5b51a651db7 173 case 'u':
Michu90 4:a5b51a651db7 174 i+=0.1;
Michu90 4:a5b51a651db7 175 break;
Michu90 4:a5b51a651db7 176
Michu90 4:a5b51a651db7 177 case 'd':
Michu90 4:a5b51a651db7 178 i-=0.1;
Michu90 4:a5b51a651db7 179 break;
Michu90 4:a5b51a651db7 180
Michu90 4:a5b51a651db7 181
Michu90 4:a5b51a651db7 182 case 'q':
Michu90 4:a5b51a651db7 183 for(i=1000;i<1500;i++){
Michu90 5:c3caf8b83e6b 184 M1.pulsewidth_us(i);
Michu90 4:a5b51a651db7 185 wait(0.005);
Michu90 4:a5b51a651db7 186 }
Michu90 4:a5b51a651db7 187 break;
Michu90 4:a5b51a651db7 188
Michu90 4:a5b51a651db7 189 case 'w':
Michu90 4:a5b51a651db7 190 for(i=1500;i>1000;i--){
Michu90 5:c3caf8b83e6b 191 M1.pulsewidth_us(i);
Michu90 4:a5b51a651db7 192 wait(0.005);
Michu90 4:a5b51a651db7 193 }
Michu90 4:a5b51a651db7 194 break;
Michu90 4:a5b51a651db7 195 case 'e':
Michu90 5:c3caf8b83e6b 196 for(i=1000;i<1500;i=i+0.1){
Michu90 5:c3caf8b83e6b 197 M1.pulsewidth_us(i);
Michu90 5:c3caf8b83e6b 198 wait(0.001);
Michu90 5:c3caf8b83e6b 199 }
Michu90 4:a5b51a651db7 200 break;
Michu90 4:a5b51a651db7 201 case 'r':
Michu90 5:c3caf8b83e6b 202 for(i=1500;i>1000;i-=0.1){
Michu90 5:c3caf8b83e6b 203 M1.pulsewidth_us(i);
Michu90 5:c3caf8b83e6b 204 wait(0.001);
Michu90 5:c3caf8b83e6b 205 }
Michu90 4:a5b51a651db7 206 break;
Michu90 4:a5b51a651db7 207 }
Michu90 4:a5b51a651db7 208
Michu90 5:c3caf8b83e6b 209 znak=0;
Michu90 5:c3caf8b83e6b 210 M1.pulsewidth_us(i);
Michu90 4:a5b51a651db7 211 }
Michu90 5:c3caf8b83e6b 212
Michu90 4:a5b51a651db7 213
Michu90 4:a5b51a651db7 214 if(bluetooth.readable()){
Michu90 4:a5b51a651db7 215
Michu90 4:a5b51a651db7 216 znak2=bluetooth.getc();
Michu90 4:a5b51a651db7 217
Michu90 5:c3caf8b83e6b 218 switch (znak2){
Michu90 4:a5b51a651db7 219 case 'a':
Michu90 5:c3caf8b83e6b 220 PWM1zad-=50;
Michu90 5:c3caf8b83e6b 221 PWM2zad-=50;
Michu90 5:c3caf8b83e6b 222 PWM3zad-=50;
Michu90 5:c3caf8b83e6b 223 PWM4zad-=50;
Michu90 5:c3caf8b83e6b 224 if(PWM1zad<10000){
Michu90 5:c3caf8b83e6b 225 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 226 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 227 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 228 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 229 }
Michu90 5:c3caf8b83e6b 230 //ustawianie
Michu90 5:c3caf8b83e6b 231 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 232 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 233 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 234 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 235 znak2=0;
Michu90 4:a5b51a651db7 236 break;
Michu90 4:a5b51a651db7 237
Michu90 4:a5b51a651db7 238 case 'b':
Michu90 5:c3caf8b83e6b 239 PWM1zad+=50;
Michu90 5:c3caf8b83e6b 240 PWM2zad+=50;
Michu90 5:c3caf8b83e6b 241 PWM3zad+=50;
Michu90 5:c3caf8b83e6b 242 PWM4zad+=50;
Michu90 5:c3caf8b83e6b 243 if(PWM1zad>=20000){
Michu90 5:c3caf8b83e6b 244 PWM1zad=20000;
Michu90 5:c3caf8b83e6b 245 PWM2zad=20000;
Michu90 5:c3caf8b83e6b 246 PWM3zad=20000;
Michu90 5:c3caf8b83e6b 247 PWM4zad=20000;
Michu90 5:c3caf8b83e6b 248 }
Michu90 5:c3caf8b83e6b 249 //ustawianie
Michu90 5:c3caf8b83e6b 250 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 251 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 252 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 253 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 254 znak2=0;
Michu90 5:c3caf8b83e6b 255 break;
Michu90 5:c3caf8b83e6b 256
Michu90 5:c3caf8b83e6b 257 case 'x':
Michu90 5:c3caf8b83e6b 258 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 259 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 260 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 261 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 262 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 263 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 264 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 265 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 266 sprintf(buff,"Odlacz silniki\n\r");
Michu90 5:c3caf8b83e6b 267 pc.printf(buff);
Michu90 5:c3caf8b83e6b 268 wait(1.0f);
Michu90 5:c3caf8b83e6b 269 sprintf(buff,"5 \n\r");
Michu90 5:c3caf8b83e6b 270 pc.printf(buff);
Michu90 5:c3caf8b83e6b 271 wait(1.0f);
Michu90 5:c3caf8b83e6b 272 sprintf(buff,"4 \n\r");
Michu90 5:c3caf8b83e6b 273 pc.printf(buff);
Michu90 5:c3caf8b83e6b 274 wait(1.0f);
Michu90 5:c3caf8b83e6b 275 sprintf(buff,"3 \n\r");
Michu90 5:c3caf8b83e6b 276 pc.printf(buff);
Michu90 5:c3caf8b83e6b 277 wait(1.0f);
Michu90 5:c3caf8b83e6b 278 sprintf(buff,"2 \n\r");
Michu90 5:c3caf8b83e6b 279 pc.printf(buff);
Michu90 5:c3caf8b83e6b 280 wait(1.0f);
Michu90 5:c3caf8b83e6b 281 sprintf(buff,"1 \n\r");
Michu90 5:c3caf8b83e6b 282 pc.printf(buff);
Michu90 5:c3caf8b83e6b 283 wait(1.0f);
Michu90 5:c3caf8b83e6b 284 sprintf(buff,"GO! \n\r");
Michu90 4:a5b51a651db7 285 pc.printf(buff);
Michu90 4:a5b51a651db7 286 break;
Michu90 4:a5b51a651db7 287
Michu90 5:c3caf8b83e6b 288 }
Michu90 5:c3caf8b83e6b 289
Michu90 4:a5b51a651db7 290 }
Igor_W 2:9e6ed6302c78 291 }
mbed_official 0:50d2b9c62765 292 }