Quadrocopter / 2015_04_17_quadro_bez_sterowania

bez sterowania

Dependencies:   FastPWM mbed-src

Fork of 2015_01_29_quadro2 by Quadrocopter

main.cpp@11:38db3ed41f13, 2015-04-17 (annotated)

Committer:
Michu90
Date:
Fri Apr 17 07:48:05 2015 +0000
Revision:
11:38db3ed41f13
Parent:
10:605b0bfadc2e 
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 9:4e94a16ca90c 12 #define filtrWymiar 5
Michu90 9:4e94a16ca90c 13 #define filtrSygnal roll
Michu90 9:4e94a16ca90c 14 #define filtr2Wymiar 5
Michu90 9:4e94a16ca90c 15 #define filtr2Sygnal pitch
Michu90 8:dc48ce79ad59 16 #define filtr3Wymiar 100
Michu90 8:dc48ce79ad59 17 #define filtr3Sygnal d[5]
Michu90 5:c3caf8b83e6b 18
Michu90 5:c3caf8b83e6b 19
Michu90 5:c3caf8b83e6b 20 //kalman
Michu90 5:c3caf8b83e6b 21 // Structs for containing filter data
Michu90 5:c3caf8b83e6b 22 kalman_data pitch_data;
Michu90 5:c3caf8b83e6b 23 kalman_data roll_data;
Michu90 5:c3caf8b83e6b 24
Michu90 5:c3caf8b83e6b 25 kalman_data pitch_data2;
Michu90 5:c3caf8b83e6b 26 kalman_data roll_data2;
Michu90 5:c3caf8b83e6b 27
Michu90 5:c3caf8b83e6b 28 DigitalOut myled(PTA2);
Michu90 5:c3caf8b83e6b 29 DigitalOut myled2(PTA1);
Michu90 5:c3caf8b83e6b 30 Serial pc(USBTX, USBRX);
Michu90 5:c3caf8b83e6b 31 Serial bluetooth(D1, D0);
Michu90 5:c3caf8b83e6b 32 IMU imu(PTE25,PTE24);
Michu90 5:c3caf8b83e6b 33 Offsets off;
Michu90 9:4e94a16ca90c 34 InterruptIn event(PTB23);
Michu90 5:c3caf8b83e6b 35
Michu90 5:c3caf8b83e6b 36 FastPWM M1(D10);
Michu90 5:c3caf8b83e6b 37 FastPWM M2(D11);
Michu90 5:c3caf8b83e6b 38 FastPWM M3(D12);
Michu90 5:c3caf8b83e6b 39 FastPWM M4(D13);
Igor_W 2:9e6ed6302c78 40
Michu90 5:c3caf8b83e6b 41 Ticker triger1; //przerwanie filtracji
Michu90 8:dc48ce79ad59 42 Ticker triger2; //kalibracja accelero
Michu90 8:dc48ce79ad59 43 Ticker triger3; //kalibracja zyro
Michu90 9:4e94a16ca90c 44 Ticker tachometer;
Igor_W 2:9e6ed6302c78 45
Michu90 5:c3caf8b83e6b 46 float d[9];
Michu90 5:c3caf8b83e6b 47 double D[9];
Michu90 5:c3caf8b83e6b 48 float o[3];
Michu90 5:c3caf8b83e6b 49 float O[3];
Michu90 5:c3caf8b83e6b 50 char buff[160];
Michu90 10:605b0bfadc2e 51 float r,katx,katy,katz;
Michu90 5:c3caf8b83e6b 52 float rbut,katxbut,katybut;
Michu90 10:605b0bfadc2e 53 float pitch, roll, yaw;
Michu90 5:c3caf8b83e6b 54 float pitch2, roll2;
Michu90 10:605b0bfadc2e 55 float pitchE, rollE, yawE;
Michu90 5:c3caf8b83e6b 56 double i;
Michu90 5:c3caf8b83e6b 57 float offsetGyro[3];
Michu90 10:605b0bfadc2e 58 float offsetMagneto;
Michu90 10:605b0bfadc2e 59 float yawOff;
Michu90 5:c3caf8b83e6b 60 char odczyt[20];
Michu90 4:a5b51a651db7 61 char znak;
Michu90 4:a5b51a651db7 62 char znak2;
Michu90 7:2ba30a0cdc16 63 float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
Michu90 6:8cc6df266363 64 float U1,U2,U3;
Michu90 6:8cc6df266363 65 float Om1,Om2,Om3,Om4;
Michu90 6:8cc6df266363 66 float wyp1,wyp2,wyp3,wyp4;
Michu90 9:4e94a16ca90c 67 int impulsA;
Michu90 5:c3caf8b83e6b 68
Michu90 5:c3caf8b83e6b 69 double PWM1zad;
Michu90 5:c3caf8b83e6b 70 double PWM2zad;
Michu90 5:c3caf8b83e6b 71 double PWM3zad;
Michu90 5:c3caf8b83e6b 72 double PWM4zad;
Michu90 5:c3caf8b83e6b 73 double valPWM1;
Michu90 5:c3caf8b83e6b 74 double valPWM2;
Michu90 5:c3caf8b83e6b 75 double valPWM3;
Michu90 5:c3caf8b83e6b 76 double valPWM4;
Michu90 8:dc48ce79ad59 77 float T1zad,T2zad,T3zad,T4zad;
Michu90 8:dc48ce79ad59 78
Michu90 8:dc48ce79ad59 79 //zmienne tymczasowe
Michu90 8:dc48ce79ad59 80 float katxzyro,katyzyro,katzzyro;
Michu90 8:dc48ce79ad59 81 float fkompl;
Michu90 8:dc48ce79ad59 82 float xmin, ymin, zmin;
Michu90 8:dc48ce79ad59 83 float xmax, ymax, zmax;
Michu90 8:dc48ce79ad59 84 float filtrBufor[filtrWymiar];
Michu90 8:dc48ce79ad59 85 int filtrZmienna=0;
Michu90 8:dc48ce79ad59 86 float filtrSuma=0;
Michu90 8:dc48ce79ad59 87 float filtrWynik;
Michu90 8:dc48ce79ad59 88 float filtr2Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 89 int filtr2Zmienna=0;
Michu90 8:dc48ce79ad59 90 float filtr2Suma=0;
Michu90 8:dc48ce79ad59 91 float filtr2Wynik;
Michu90 8:dc48ce79ad59 92 float filtr3Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 93 int filtr3Zmienna=0;
Michu90 8:dc48ce79ad59 94 float filtr3Suma=0;
Michu90 8:dc48ce79ad59 95 float filtr3Wynik;
Michu90 8:dc48ce79ad59 96
Michu90 8:dc48ce79ad59 97 float kalmanpitchzyro;
Michu90 9:4e94a16ca90c 98 float kalmanpitchzyrobut;
Michu90 8:dc48ce79ad59 99 float kalmanrollzyro;
Michu90 8:dc48ce79ad59 100 float kalmanrollzyrobut;
Michu90 8:dc48ce79ad59 101 float kalmanpitchdryf;
Michu90 5:c3caf8b83e6b 102
Michu90 9:4e94a16ca90c 103 int RPMtach;
Michu90 9:4e94a16ca90c 104 float PWM4zadBuf[5];
Michu90 9:4e94a16ca90c 105 float PWM4zadPoprz;
Michu90 9:4e94a16ca90c 106 float PWM4zadWyn;
Michu90 9:4e94a16ca90c 107 float RPMtachBuf[5];
Michu90 9:4e94a16ca90c 108 int RPMtachPoprz;
Michu90 9:4e94a16ca90c 109 int RPMtachAktualny;
Michu90 9:4e94a16ca90c 110 float RPMtachWyn;
Michu90 9:4e94a16ca90c 111 int RPMtachWynPoprz;
Michu90 9:4e94a16ca90c 112 int iTach;
Michu90 9:4e94a16ca90c 113 bool TachFlaga;
Michu90 10:605b0bfadc2e 114 bool FlagaPom;
Michu90 10:605b0bfadc2e 115
Michu90 10:605b0bfadc2e 116 int Omegazad[4];
Michu90 10:605b0bfadc2e 117 int Omegamax[4][9];
Michu90 10:605b0bfadc2e 118 float a1buf[4][9];
Michu90 10:605b0bfadc2e 119 float a2buf[4][9];
Michu90 10:605b0bfadc2e 120 int ister;
Michu90 10:605b0bfadc2e 121 int jster;
Michu90 10:605b0bfadc2e 122 bool flagaster;
Michu90 10:605b0bfadc2e 123 float a1[4];
Michu90 10:605b0bfadc2e 124 float a2[4];
Michu90 9:4e94a16ca90c 125
Michu90 9:4e94a16ca90c 126 void rise(void)
Michu90 9:4e94a16ca90c 127 {
Michu90 9:4e94a16ca90c 128 //myled = !myled;
Michu90 9:4e94a16ca90c 129 if(TachFlaga==1)impulsA++;
Michu90 9:4e94a16ca90c 130 }
Michu90 9:4e94a16ca90c 131
Michu90 9:4e94a16ca90c 132
Michu90 5:c3caf8b83e6b 133 void task1()
Michu90 5:c3caf8b83e6b 134 {
Michu90 7:2ba30a0cdc16 135 //myled = !myled;
Michu90 5:c3caf8b83e6b 136 imu.readData(d);
Michu90 5:c3caf8b83e6b 137 imu.filterData(d, D);
Michu90 5:c3caf8b83e6b 138 off.offsetData(d,offsetGyro,o);
Michu90 5:c3caf8b83e6b 139 off.offsetData2(D,offsetGyro,O);
Michu90 5:c3caf8b83e6b 140
Michu90 5:c3caf8b83e6b 141 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 5:c3caf8b83e6b 142 katx = acos(d[4]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 143 katy = acos(d[3]/r)-M_PI2;
Michu90 10:605b0bfadc2e 144
Michu90 10:605b0bfadc2e 145 yaw = atan2(d[7],d[6])+4.98333*M_PI/180;
Michu90 10:605b0bfadc2e 146 katz = katz+(o[2]*dt);
Michu90 10:605b0bfadc2e 147 off.offsetMagneto(&yaw,&offsetMagneto,&yawOff);
Michu90 10:605b0bfadc2e 148 // Correct for heading < 0deg and heading > 360deg
Michu90 10:605b0bfadc2e 149 /*if(yawOff < 0){
Michu90 10:605b0bfadc2e 150 yawOff+= 2 * M_PI;
Michu90 10:605b0bfadc2e 151 }
Michu90 10:605b0bfadc2e 152
Michu90 10:605b0bfadc2e 153 if(yawOff > 2 * M_PI){
Michu90 10:605b0bfadc2e 154 yawOff-= 2 * M_PI;
Michu90 10:605b0bfadc2e 155 }*/
Michu90 5:c3caf8b83e6b 156
Michu90 9:4e94a16ca90c 157 /*rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
Michu90 5:c3caf8b83e6b 158 katxbut = acos(D[4]/rbut)-M_PI2;
Michu90 9:4e94a16ca90c 159 katybut = acos(D[3]/rbut)-M_PI2;*/
Michu90 5:c3caf8b83e6b 160
Michu90 8:dc48ce79ad59 161 //katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 162 //fkompl = katyzyro*0.95+ (-katy)*0.05;
Michu90 5:c3caf8b83e6b 163
Michu90 5:c3caf8b83e6b 164 //Filtr Kalmana
Michu90 5:c3caf8b83e6b 165 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 5:c3caf8b83e6b 166 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 9:4e94a16ca90c 167 pitch = -pitch_data.x1;
Michu90 9:4e94a16ca90c 168 kalmanpitchzyro = pitch_data.x2;
Michu90 5:c3caf8b83e6b 169 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 170 kalmanrollzyro = roll_data.x2;
Michu90 9:4e94a16ca90c 171
Michu90 9:4e94a16ca90c 172 /*
Michu90 5:c3caf8b83e6b 173 //Filtr Kalmana butterworth 2nd
Michu90 5:c3caf8b83e6b 174 kalman_innovate(&pitch_data2, katxbut, O[0]);
Michu90 5:c3caf8b83e6b 175 kalman_innovate(&roll_data2, -katybut, O[1]);
Michu90 5:c3caf8b83e6b 176 pitch2 = pitch_data2.x1;
Michu90 9:4e94a16ca90c 177 kalmanpitchzyrobut = pitch_data2.x2;
Michu90 5:c3caf8b83e6b 178 roll2 = roll_data2.x1;
Michu90 9:4e94a16ca90c 179 kalmanrollzyrobut = roll_data2.x2;*/
Michu90 9:4e94a16ca90c 180
Michu90 9:4e94a16ca90c 181 if (filtrZmienna < 0) {
Michu90 9:4e94a16ca90c 182 filtrWynik = filtrSygnal;
Michu90 9:4e94a16ca90c 183 filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
Michu90 9:4e94a16ca90c 184 filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
Michu90 9:4e94a16ca90c 185 } else {
Michu90 9:4e94a16ca90c 186 if (filtrZmienna == filtrWymiar - 1) {
Michu90 9:4e94a16ca90c 187 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 188 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 9:4e94a16ca90c 189 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 190 filtrWynik = filtrSuma / filtrWymiar;
Michu90 9:4e94a16ca90c 191 } else {
Michu90 9:4e94a16ca90c 192 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 193 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 9:4e94a16ca90c 194 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 9:4e94a16ca90c 195 filtrWynik = filtrSuma / filtrWymiar;
Michu90 9:4e94a16ca90c 196 }
Michu90 9:4e94a16ca90c 197 }
Michu90 9:4e94a16ca90c 198 filtrZmienna++;
Michu90 9:4e94a16ca90c 199 if (filtrZmienna == filtrWymiar) {
Michu90 9:4e94a16ca90c 200 filtrZmienna = 0;
Michu90 9:4e94a16ca90c 201 }
Michu90 9:4e94a16ca90c 202
Michu90 9:4e94a16ca90c 203 roll = filtrWynik;
Michu90 9:4e94a16ca90c 204
Michu90 9:4e94a16ca90c 205 if (filtr2Zmienna < 0) {
Michu90 9:4e94a16ca90c 206 filtr2Wynik = filtr2Sygnal;
Michu90 9:4e94a16ca90c 207 filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 208 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
Michu90 9:4e94a16ca90c 209 } else {
Michu90 9:4e94a16ca90c 210 if (filtr2Zmienna == filtr2Wymiar - 1) {
Michu90 9:4e94a16ca90c 211 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 212 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 213 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 214 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 9:4e94a16ca90c 215 } else {
Michu90 9:4e94a16ca90c 216 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 217 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 9:4e94a16ca90c 218 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 9:4e94a16ca90c 219 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 9:4e94a16ca90c 220 }
Michu90 9:4e94a16ca90c 221 }
Michu90 9:4e94a16ca90c 222 filtr2Zmienna++;
Michu90 9:4e94a16ca90c 223 if (filtr2Zmienna == filtr2Wymiar) {
Michu90 9:4e94a16ca90c 224 filtr2Zmienna = 0;
Michu90 9:4e94a16ca90c 225 }
Michu90 9:4e94a16ca90c 226 pitch=filtr2Wynik;
Michu90 5:c3caf8b83e6b 227
Michu90 7:2ba30a0cdc16 228 //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
Michu90 7:2ba30a0cdc16 229 //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
Michu90 7:2ba30a0cdc16 230 //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
Michu90 6:8cc6df266363 231
Michu90 7:2ba30a0cdc16 232 //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 233 //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 234 //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 235 //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
Michu90 6:8cc6df266363 236
Michu90 7:2ba30a0cdc16 237 /*wyp1 = sqrt(Om1)*13.17523+10000;
Michu90 6:8cc6df266363 238 wyp2 = sqrt(Om2)*13.17523+10000;
Michu90 6:8cc6df266363 239 wyp3 = sqrt(Om3)*13.17523+10000;
Michu90 7:2ba30a0cdc16 240 wyp4 = sqrt(Om4)*13.17523+10000;*/
Michu90 6:8cc6df266363 241
Michu90 6:8cc6df266363 242
Michu90 7:2ba30a0cdc16 243 // *********************** DISCRETE PID CONTROLLER **********************
Michu90 11:38db3ed41f13 244 //stale
Michu90 7:2ba30a0cdc16 245 //b=0.000015
Michu90 7:2ba30a0cdc16 246 //B1=0.000776646
Michu90 7:2ba30a0cdc16 247 //C1=-0.334958973
Michu90 7:2ba30a0cdc16 248 //l=0.3
Michu90 7:2ba30a0cdc16 249 //d=0.000033
Michu90 7:2ba30a0cdc16 250 //4d=0.000132
Michu90 9:4e94a16ca90c 251
Michu90 9:4e94a16ca90c 252 //B1/b1=50.22498189
Michu90 9:4e94a16ca90c 253 //C1/b1=-21611.4954
Michu90 9:4e94a16ca90c 254 //b1/B1=0.01991041
Michu90 7:2ba30a0cdc16 255 //C1/B1=-431.2892625
Michu90 8:dc48ce79ad59 256 //1/B1=1287.588322
Michu90 8:dc48ce79ad59 257
Michu90 9:4e94a16ca90c 258 //2b2l=0.000009424
Michu90 9:4e94a16ca90c 259 //B2/b2=51.130533
Michu90 9:4e94a16ca90c 260 //C2/b2=-30717.00651
Michu90 9:4e94a16ca90c 261 //b2/B2=0.019557786
Michu90 9:4e94a16ca90c 262 //C2/B2=-600.7566265
Michu90 7:2ba30a0cdc16 263
Michu90 9:4e94a16ca90c 264 //2b3l=0.000009139
Michu90 9:4e94a16ca90c 265 //B3/b3=50.66613192
Michu90 9:4e94a16ca90c 266 //C3/b3=-26072.99576
Michu90 9:4e94a16ca90c 267 //b3/B3=0.01973705
Michu90 7:2ba30a0cdc16 268 //C3/B3=-514.6040317
Michu90 8:dc48ce79ad59 269
Michu90 9:4e94a16ca90c 270 //2b4l=0.000009276
Michu90 9:4e94a16ca90c 271 //B4/b4=50.95680893
Michu90 9:4e94a16ca90c 272 //C4/b4=-28979.76588
Michu90 9:4e94a16ca90c 273 //b4/B4=0.019624463
Michu90 9:4e94a16ca90c 274 //C4/B4=-568.7123367
Michu90 9:4e94a16ca90c 275
Michu90 11:38db3ed41f13 276 //stare 2b*l
Michu90 11:38db3ed41f13 277 //0.0000092780
Michu90 11:38db3ed41f13 278 //0.000009424
Michu90 11:38db3ed41f13 279 //0.000009139
Michu90 11:38db3ed41f13 280 //0.000009276
Michu90 11:38db3ed41f13 281
Michu90 11:38db3ed41f13 282 //2b1l=0.000000096789585
Michu90 11:38db3ed41f13 283 //2b2l=0.000000095889323
Michu90 11:38db3ed41f13 284 //2b3l=0.000000097529031
Michu90 11:38db3ed41f13 285 //2b4l=0.000000093579744
Michu90 11:38db3ed41f13 286
Michu90 11:38db3ed41f13 287 // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
Michu90 11:38db3ed41f13 288 // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
Michu90 11:38db3ed41f13 289 // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
Michu90 11:38db3ed41f13 290 //omega1^2=(B1/b1*PWM1zad+C1/b1)-U2/2b1l+U3/4d
Michu90 11:38db3ed41f13 291 //omega2^2=(B2/b2*PWM2zad+C2/b2)+U1/2b2l-U3/4d
Michu90 11:38db3ed41f13 292 //omega3^2=(B3/b3*PWM3zad+C3/b3)+U2/2b3l+U3/4d
Michu90 11:38db3ed41f13 293 //omega4^2=(B4/b4*PWM4zad+C4/b4)-U1/2b4l-U3/4d
Michu90 11:38db3ed41f13 294 //wyp1=b/B1*omega1^2-C1/B1
Michu90 11:38db3ed41f13 295 //wyp2=b/B*omega2^2-C/B
Michu90 11:38db3ed41f13 296 //wyp3=b/B3*omega3^2-C3/B3
Michu90 11:38db3ed41f13 297 //wyp4=b/B*omega4^2-C/B
Michu90 11:38db3ed41f13 298
Michu90 8:dc48ce79ad59 299 //B*PWMzad+C=T
Michu90 8:dc48ce79ad59 300 //PWMzad=1/B*T-C/B
Michu90 8:dc48ce79ad59 301 /*
Michu90 8:dc48ce79ad59 302 PWMzad1=1287.588322*T1zad-(-431.2892625);
Michu90 9:4e94a16ca90c 303 PWMzad2=1245.207965*T2zad-(-600.7566265);
Michu90 9:4e94a16ca90c 304 PWMzad3=1295.740776*T3zad-(-514.6040317);
Michu90 9:4e94a16ca90c 305 PWMzad4=1269.412072*T4zad-(-568.7123367);*/
Michu90 11:38db3ed41f13 306 /*
Michu90 11:38db3ed41f13 307 wyp1=0.01991041*Om1-(-431.2892625)+PWM1zad;
Michu90 11:38db3ed41f13 308 wyp2=0.019557786*Om2-(-600.7566265)+PWM2zad;
Michu90 11:38db3ed41f13 309 wyp3=0.01973705*Om3-(-514.6040317)+PWM3zad;
Michu90 11:38db3ed41f13 310 wyp4=0.019624463*Om4-(-568.7123367)+PWM4zad;
Michu90 11:38db3ed41f13 311 */
Michu90 9:4e94a16ca90c 312
Michu90 9:4e94a16ca90c 313 pitchE=pitchE+(0-pitch);
Michu90 7:2ba30a0cdc16 314 if(pitchE>3) pitchE=3;
Michu90 9:4e94a16ca90c 315 if(pitchE<-3) pitchE=-3;
Michu90 8:dc48ce79ad59 316 //Kd1=Kp1*Td1/T;
Michu90 8:dc48ce79ad59 317 //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
Michu90 6:8cc6df266363 318
Michu90 7:2ba30a0cdc16 319 rollE=rollE+(0-roll2);
Michu90 8:dc48ce79ad59 320 if(rollE>5) rollE=5;
Michu90 8:dc48ce79ad59 321 if(rollE<-5) rollE=-5;
Michu90 8:dc48ce79ad59 322 //Kd2=Kp2*Td2/T;
Michu90 8:dc48ce79ad59 323 //if(Ti2==0){Ki2=0;}else Ki2=Kp2*T/Ti2;
Michu90 7:2ba30a0cdc16 324
Michu90 7:2ba30a0cdc16 325 /* yawE=yawE+(0-roll2);
Michu90 7:2ba30a0cdc16 326 if(yawE>3) yawE=3;*/
Michu90 7:2ba30a0cdc16 327 Kd3=Kp3*Td3/T;
Michu90 8:dc48ce79ad59 328 //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
Michu90 7:2ba30a0cdc16 329
Michu90 9:4e94a16ca90c 330 //U1 = 0.0346*(Kp1*(0-pitch)+Kd1*(0-(-kalmanpitchzyro))+Ki1*pitchE);
Michu90 9:4e94a16ca90c 331 //U2 = 0.0338*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
Michu90 9:4e94a16ca90c 332 //U3 = 0.0666*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
Michu90 11:38db3ed41f13 333
Michu90 11:38db3ed41f13 334 U1 = 0.0173*(Kp1*(0-pitch)+Kd1*(0+kalmanpitchzyro)+Ki1*pitchE);
Michu90 11:38db3ed41f13 335 U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-kalmanrollzyro)+Ki2*rollE);
Michu90 11:38db3ed41f13 336 U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-o[2]));//+Ki3*rollE);
Michu90 7:2ba30a0cdc16 337
Michu90 11:38db3ed41f13 338 Om1 = Omegazad[0]*Omegazad[0]-U2/0.000000096789585 + U3/0.000132 ; //kwadraty
Michu90 11:38db3ed41f13 339 Om2 = Omegazad[1]*Omegazad[1]+U1/0.000000095889323 - U3/0.000132 ;
Michu90 11:38db3ed41f13 340 Om3 = Omegazad[2]*Omegazad[2]+U2/0.000000097529031 + U3/0.000132 ;
Michu90 11:38db3ed41f13 341 Om4 = Omegazad[3]*Omegazad[3]-U1/0.000000093579744 - U3/0.000132 ;
Michu90 11:38db3ed41f13 342
Michu90 11:38db3ed41f13 343 wyp1=(sqrt(Om1)-a2[0])/a1[0];
Michu90 11:38db3ed41f13 344 wyp2=(sqrt(Om2)-a2[1])/a1[1];
Michu90 11:38db3ed41f13 345 wyp3=(sqrt(Om3)-a2[2])/a1[2];
Michu90 11:38db3ed41f13 346 wyp4=(sqrt(Om4)-a2[3])/a1[3];
Michu90 7:2ba30a0cdc16 347
Michu90 6:8cc6df266363 348 if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
Michu90 6:8cc6df266363 349 if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
Michu90 6:8cc6df266363 350 if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
Michu90 6:8cc6df266363 351
Michu90 9:4e94a16ca90c 352 if(wyp2<=10001 || wyp2>40001) valPWM2=10000;
Michu90 9:4e94a16ca90c 353 if(wyp2>=20000 && wyp2<40000) valPWM2=20000;
Michu90 9:4e94a16ca90c 354 if(wyp2>10001 && wyp2<20000) valPWM2=(int)wyp2;
Michu90 6:8cc6df266363 355
Michu90 9:4e94a16ca90c 356 if(wyp3<=10001 || wyp3>40001) valPWM3=10000;
Michu90 9:4e94a16ca90c 357 if(wyp3>=20000 && wyp3<40000) valPWM3=20000;
Michu90 9:4e94a16ca90c 358 if(wyp3>10001 && wyp3<20000) valPWM3=(int)wyp3;
Michu90 6:8cc6df266363 359
Michu90 9:4e94a16ca90c 360 if(wyp4<=10001 || wyp4>40001) valPWM4=10000;
Michu90 9:4e94a16ca90c 361 if(wyp4>=20000 && wyp4<40000) valPWM4=20000;
Michu90 9:4e94a16ca90c 362 if(wyp4>10001 && wyp4<20000) valPWM4=(int)wyp4;
Michu90 6:8cc6df266363 363
Michu90 5:c3caf8b83e6b 364
Michu90 5:c3caf8b83e6b 365 //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 366 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 367 //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 368 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 369 //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 370 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 371 //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 372 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 373
Michu90 6:8cc6df266363 374 M1.pulsewidth_us(valPWM1);
Michu90 6:8cc6df266363 375 M2.pulsewidth_us(valPWM2);
Michu90 6:8cc6df266363 376 M3.pulsewidth_us(valPWM3);
Michu90 6:8cc6df266363 377 M4.pulsewidth_us(valPWM4);
Michu90 6:8cc6df266363 378
Michu90 7:2ba30a0cdc16 379 //myled = !myled;
Michu90 5:c3caf8b83e6b 380 }
Michu90 5:c3caf8b83e6b 381
Michu90 9:4e94a16ca90c 382 /*
Michu90 5:c3caf8b83e6b 383 void task2()
Michu90 5:c3caf8b83e6b 384 {
Michu90 9:4e94a16ca90c 385
Michu90 5:c3caf8b83e6b 386 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 387 pc.printf(buff);
Michu90 9:4e94a16ca90c 388 myled2 = !myled2;
Michu90 8:dc48ce79ad59 389 imu.readData(d);
Michu90 8:dc48ce79ad59 390
Michu90 8:dc48ce79ad59 391 if (filtrZmienna < 0) {
Michu90 8:dc48ce79ad59 392 filtrWynik = filtrSygnal;
Michu90 8:dc48ce79ad59 393 filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
Michu90 8:dc48ce79ad59 394 filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
Michu90 8:dc48ce79ad59 395 } else {
Michu90 8:dc48ce79ad59 396 if (filtrZmienna == filtrWymiar - 1) {
Michu90 8:dc48ce79ad59 397 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 398 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 399 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 400 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 401 } else {
Michu90 8:dc48ce79ad59 402 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 403 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 404 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 405 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 406 }
Michu90 8:dc48ce79ad59 407 }
Michu90 8:dc48ce79ad59 408 filtrZmienna++;
Michu90 8:dc48ce79ad59 409 if (filtrZmienna == filtrWymiar) {
Michu90 8:dc48ce79ad59 410 filtrZmienna = 0;
Michu90 8:dc48ce79ad59 411 }
Michu90 8:dc48ce79ad59 412
Michu90 8:dc48ce79ad59 413 if (filtrWynik < xmin) xmin=filtrWynik;
Michu90 8:dc48ce79ad59 414 if (filtrWynik > xmax) xmax=filtrWynik;
Michu90 8:dc48ce79ad59 415
Michu90 8:dc48ce79ad59 416
Michu90 8:dc48ce79ad59 417 if (filtr2Zmienna < 0) {
Michu90 8:dc48ce79ad59 418 filtr2Wynik = filtr2Sygnal;
Michu90 8:dc48ce79ad59 419 filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 420 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
Michu90 8:dc48ce79ad59 421 } else {
Michu90 8:dc48ce79ad59 422 if (filtr2Zmienna == filtr2Wymiar - 1) {
Michu90 8:dc48ce79ad59 423 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 424 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 425 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 426 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 427 } else {
Michu90 8:dc48ce79ad59 428 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 429 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 430 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 431 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 432 }
Michu90 8:dc48ce79ad59 433 }
Michu90 8:dc48ce79ad59 434 filtr2Zmienna++;
Michu90 8:dc48ce79ad59 435 if (filtr2Zmienna == filtr2Wymiar) {
Michu90 8:dc48ce79ad59 436 filtr2Zmienna = 0;
Michu90 8:dc48ce79ad59 437 }
Michu90 8:dc48ce79ad59 438
Michu90 8:dc48ce79ad59 439 if (filtr2Wynik < ymin) ymin=filtr2Wynik;
Michu90 8:dc48ce79ad59 440 if (filtr2Wynik > ymax) ymax=filtr2Wynik;
Michu90 8:dc48ce79ad59 441
Michu90 8:dc48ce79ad59 442 if (filtr3Zmienna < 0) {
Michu90 8:dc48ce79ad59 443 filtr3Wynik = filtr3Sygnal;
Michu90 8:dc48ce79ad59 444 filtr3Bufor[filtr3Zmienna + filtr3Wymiar] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 445 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna + filtr3Wymiar];
Michu90 8:dc48ce79ad59 446 } else {
Michu90 8:dc48ce79ad59 447 if (filtr3Zmienna == filtr3Wymiar - 1) {
Michu90 8:dc48ce79ad59 448 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 449 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 450 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 451 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 452 } else {
Michu90 8:dc48ce79ad59 453 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 454 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 455 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 456 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 457 }
Michu90 8:dc48ce79ad59 458 }
Michu90 8:dc48ce79ad59 459 filtr3Zmienna++;
Michu90 8:dc48ce79ad59 460 if (filtr3Zmienna == filtr3Wymiar) {
Michu90 8:dc48ce79ad59 461 filtr3Zmienna = 0;
Michu90 8:dc48ce79ad59 462 }
Michu90 8:dc48ce79ad59 463
Michu90 8:dc48ce79ad59 464 if (filtr3Wynik < zmin) zmin=filtr3Wynik;
Michu90 8:dc48ce79ad59 465 if (filtr3Wynik > zmax) zmax=filtr3Wynik;
Michu90 8:dc48ce79ad59 466
Michu90 8:dc48ce79ad59 467 //sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
Michu90 8:dc48ce79ad59 468 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 469 }
Michu90 5:c3caf8b83e6b 470
Michu90 8:dc48ce79ad59 471
Michu90 8:dc48ce79ad59 472 void task3()
Michu90 8:dc48ce79ad59 473 {
Michu90 8:dc48ce79ad59 474 imu.readData(d);
Michu90 8:dc48ce79ad59 475 off.offsetData(d,offsetGyro,o);
Michu90 8:dc48ce79ad59 476 katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 477 katyzyro = katyzyro + o[1]*dt;
Michu90 8:dc48ce79ad59 478 katzzyro = katzzyro + o[2]*dt;
Michu90 8:dc48ce79ad59 479
Michu90 8:dc48ce79ad59 480 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 8:dc48ce79ad59 481 katx = acos(d[4]/r)-M_PI2;
Michu90 8:dc48ce79ad59 482 katy = acos(d[3]/r)-M_PI2;
Michu90 8:dc48ce79ad59 483
Michu90 8:dc48ce79ad59 484 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 8:dc48ce79ad59 485 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 8:dc48ce79ad59 486 pitch = pitch_data.x1;
Michu90 8:dc48ce79ad59 487 kalmanpitchzyro = pitch_data.x2;
Michu90 8:dc48ce79ad59 488 kalmanpitchdryf = pitch_data.x3;
Michu90 8:dc48ce79ad59 489 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 490 }
Michu90 9:4e94a16ca90c 491 */
Michu90 9:4e94a16ca90c 492
Michu90 9:4e94a16ca90c 493 void task_tachometer()
Michu90 9:4e94a16ca90c 494 {
Michu90 10:605b0bfadc2e 495
Michu90 9:4e94a16ca90c 496 TachFlaga=0;
Michu90 10:605b0bfadc2e 497 if(FlagaPom==1){
Michu90 10:605b0bfadc2e 498 PWM4zad+=5;
Michu90 10:605b0bfadc2e 499 M3.pulsewidth_us(PWM4zad);
Michu90 10:605b0bfadc2e 500 }
Michu90 10:605b0bfadc2e 501 wait(0.5f);
Michu90 9:4e94a16ca90c 502 TachFlaga=1;
Michu90 9:4e94a16ca90c 503 //myled2=!myled2;
Michu90 9:4e94a16ca90c 504 RPMtach=impulsA*10; // dla T=3s
Michu90 9:4e94a16ca90c 505 //RPMtach=impulsA*6; //dla T=5s
Michu90 9:4e94a16ca90c 506 //RPMtach=impulsA*3; //dla T=10s
Michu90 9:4e94a16ca90c 507 //RPMtachAktualny=impulsA*3;
Michu90 9:4e94a16ca90c 508 /*
Michu90 9:4e94a16ca90c 509 if (TachFlaga==1){
Michu90 9:4e94a16ca90c 510 RPMtachBuf[iTach]=RPMtachAktualny;
Michu90 9:4e94a16ca90c 511 iTach++;
Michu90 9:4e94a16ca90c 512 if (iTach>3){
Michu90 9:4e94a16ca90c 513 RPMtachWyn=(RPMtachBuf[iTach-1]+RPMtachBuf[iTach-2]+RPMtachBuf[iTach-3])/3;
Michu90 9:4e94a16ca90c 514 RPMtachWynPoprz=RPMtachWyn;
Michu90 9:4e94a16ca90c 515 PWM4zadWyn=PWM4zad;
Michu90 9:4e94a16ca90c 516 TachFlaga=0;
Michu90 9:4e94a16ca90c 517 iTach=0;
Michu90 9:4e94a16ca90c 518 RPMtach=RPMtachAktualny;
Michu90 9:4e94a16ca90c 519 }
Michu90 9:4e94a16ca90c 520 }else{
Michu90 9:4e94a16ca90c 521 if(abs(PWM4zadPoprz-PWM4zad)>0 && abs(RPMtachAktualny-RPMtachWynPoprz)>20){
Michu90 9:4e94a16ca90c 522 TachFlaga=1;
Michu90 9:4e94a16ca90c 523 }
Michu90 9:4e94a16ca90c 524 //RPMtach=RPMtachAktualny;
Michu90 9:4e94a16ca90c 525 PWM4zadPoprz=PWM4zad;
Michu90 9:4e94a16ca90c 526 PWM4zad+=1;
Michu90 9:4e94a16ca90c 527 M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 528 //if(PWM4zad>14000)PWM4zad=10000;
Michu90 9:4e94a16ca90c 529 }
Michu90 9:4e94a16ca90c 530 sprintf(buff, "%f,%i,%f,%f\n\r", PWM4zad,RPMtachAktualny,PWM4zadWyn,RPMtachWyn);
Michu90 9:4e94a16ca90c 531 pc.printf(buff);
Michu90 9:4e94a16ca90c 532 */
Michu90 9:4e94a16ca90c 533 sprintf(buff, "%f,%i\n\r", PWM4zad,RPMtach);
Michu90 9:4e94a16ca90c 534 pc.printf(buff);
Michu90 9:4e94a16ca90c 535 //iTach++;
Michu90 9:4e94a16ca90c 536 //if(iTach%2>0)PWM4zad+=1;
Michu90 9:4e94a16ca90c 537 //M4.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 538
Michu90 9:4e94a16ca90c 539 //RPMtachWyn=0;
Michu90 9:4e94a16ca90c 540 //PWM4zadWyn=0;
Michu90 9:4e94a16ca90c 541 impulsA=0;
Michu90 9:4e94a16ca90c 542 }
Michu90 5:c3caf8b83e6b 543
Michu90 4:a5b51a651db7 544
mbed_official 0:50d2b9c62765 545 int main() {
Igor_W 2:9e6ed6302c78 546
Michu90 4:a5b51a651db7 547 pc.baud(115200);
Michu90 4:a5b51a651db7 548 bluetooth.baud(19200);
Michu90 5:c3caf8b83e6b 549 imu.init();
Michu90 5:c3caf8b83e6b 550 kalman_init(&pitch_data);
Michu90 5:c3caf8b83e6b 551 kalman_init(&roll_data);
Michu90 5:c3caf8b83e6b 552 kalman_init(&pitch_data2);
Michu90 5:c3caf8b83e6b 553 kalman_init(&roll_data2);
Michu90 10:605b0bfadc2e 554 //event.rise(&rise);
Michu90 10:605b0bfadc2e 555 //event.mode(PullUp);
Michu90 10:605b0bfadc2e 556 //event.enable_irq();
Igor_W 3:1425359662e4 557
Michu90 4:a5b51a651db7 558 sprintf(buff, "Hello: \n\r");
Michu90 4:a5b51a651db7 559 pc.printf(buff);
Michu90 4:a5b51a651db7 560
Michu90 11:38db3ed41f13 561 off.setOffsets(offsetGyro,&offsetMagneto, bluetooth, imu);
Michu90 5:c3caf8b83e6b 562 i=1000;
Michu90 5:c3caf8b83e6b 563
Michu90 5:c3caf8b83e6b 564 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 565 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 566 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 567 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 568
Michu90 5:c3caf8b83e6b 569 M1.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 570 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 571 M2.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 572 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 573 M3.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 574 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 575 M4.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 576 M4.pulsewidth_us(PWM4zad);
Michu90 4:a5b51a651db7 577
Michu90 9:4e94a16ca90c 578 /*Kp1=10;
Michu90 9:4e94a16ca90c 579 Kp2=8;
Michu90 9:4e94a16ca90c 580 Kp3=0;
Michu90 9:4e94a16ca90c 581 Kd1=8;
Michu90 9:4e94a16ca90c 582 Kd2=16;
Michu90 9:4e94a16ca90c 583 Kd3=5000;
Michu90 9:4e94a16ca90c 584 Ki1=0.1;
Michu90 9:4e94a16ca90c 585 Ki2=0.2;
Michu90 9:4e94a16ca90c 586 Ki3=0;
Michu90 9:4e94a16ca90c 587 Td1=0;
Michu90 9:4e94a16ca90c 588 Td2=0;
Michu90 9:4e94a16ca90c 589 Td3=0;
Michu90 9:4e94a16ca90c 590 Ti1=0;
Michu90 9:4e94a16ca90c 591 Ti2=0;
Michu90 9:4e94a16ca90c 592 Ti3=0;
Michu90 9:4e94a16ca90c 593 T=0.005;*/
Michu90 11:38db3ed41f13 594 Kp1=12;
Michu90 11:38db3ed41f13 595 Kp2=12;
Michu90 6:8cc6df266363 596 Kp3=0;
Michu90 11:38db3ed41f13 597 Kd1=2;
Michu90 11:38db3ed41f13 598 Kd2=2;
Michu90 8:dc48ce79ad59 599 Kd3=0;
Michu90 8:dc48ce79ad59 600 Ki1=0;
Michu90 8:dc48ce79ad59 601 Ki2=0;
Michu90 8:dc48ce79ad59 602 Ki3=0;
Michu90 7:2ba30a0cdc16 603 Td1=0;
Michu90 7:2ba30a0cdc16 604 Td2=0;
Michu90 7:2ba30a0cdc16 605 Td3=0;
Michu90 7:2ba30a0cdc16 606 Ti1=0;
Michu90 7:2ba30a0cdc16 607 Ti2=0;
Michu90 7:2ba30a0cdc16 608 Ti3=0;
Michu90 7:2ba30a0cdc16 609 T=0.005;
Michu90 6:8cc6df266363 610
Michu90 8:dc48ce79ad59 611 rollE=0;
Michu90 8:dc48ce79ad59 612
Michu90 8:dc48ce79ad59 613 katxzyro = 0;
Michu90 8:dc48ce79ad59 614 katyzyro = 0;
Michu90 8:dc48ce79ad59 615 katzzyro = 0;
Michu90 6:8cc6df266363 616
Michu90 9:4e94a16ca90c 617 iTach=0;
Michu90 9:4e94a16ca90c 618 TachFlaga=0;
Michu90 9:4e94a16ca90c 619
Michu90 10:605b0bfadc2e 620 a1buf[0][0]= 1.438602213;
Michu90 10:605b0bfadc2e 621 a1buf[0][1]= 1.130064065;
Michu90 10:605b0bfadc2e 622 a1buf[0][2]= 0.958928363;
Michu90 10:605b0bfadc2e 623 a1buf[0][3]= 0.836156086;
Michu90 10:605b0bfadc2e 624 a1buf[0][4]= 0.735165987;
Michu90 10:605b0bfadc2e 625 a1buf[0][5]= 0.619988352;
Michu90 10:605b0bfadc2e 626 a1buf[0][6]= 0.580966803;
Michu90 10:605b0bfadc2e 627 a1buf[0][7]= 0.580966803;
Michu90 10:605b0bfadc2e 628 a1buf[0][8]= 0.580966803;
Michu90 10:605b0bfadc2e 629 a1buf[1][0]= 1.55790332;
Michu90 10:605b0bfadc2e 630 a1buf[1][1]= 1.181479324;
Michu90 10:605b0bfadc2e 631 a1buf[1][2]= 1.001071637;
Michu90 10:605b0bfadc2e 632 a1buf[1][3]= 0.872754805;
Michu90 10:605b0bfadc2e 633 a1buf[1][4]= 0.699592312;
Michu90 10:605b0bfadc2e 634 a1buf[1][5]= 0.644682586;
Michu90 10:605b0bfadc2e 635 a1buf[1][6]= 0.596575422;
Michu90 10:605b0bfadc2e 636 a1buf[1][7]= 0.596575422;
Michu90 10:605b0bfadc2e 637 a1buf[1][8]= 0.596575422;
Michu90 10:605b0bfadc2e 638 a1buf[2][0]= 1.475317414;
Michu90 10:605b0bfadc2e 639 a1buf[2][1]= 1.159883518;
Michu90 10:605b0bfadc2e 640 a1buf[2][2]= 0.965894001;
Michu90 10:605b0bfadc2e 641 a1buf[2][3]= 0.85553873;
Michu90 10:605b0bfadc2e 642 a1buf[2][4]= 0.738311008;
Michu90 10:605b0bfadc2e 643 a1buf[2][5]= 0.627093768;
Michu90 10:605b0bfadc2e 644 a1buf[2][6]= 0.627093768;
Michu90 10:605b0bfadc2e 645 a1buf[2][7]= 0.627093768;
Michu90 10:605b0bfadc2e 646 a1buf[2][8]= 0.627093768;
Michu90 10:605b0bfadc2e 647 a1buf[3][0]= 1.40570763;
Michu90 10:605b0bfadc2e 648 a1buf[3][1]= 1.143366337;
Michu90 10:605b0bfadc2e 649 a1buf[3][2]= 0.953383809;
Michu90 10:605b0bfadc2e 650 a1buf[3][3]= 0.850413512;
Michu90 10:605b0bfadc2e 651 a1buf[3][4]= 0.734304019;
Michu90 10:605b0bfadc2e 652 a1buf[3][5]= 0.634804892;
Michu90 10:605b0bfadc2e 653 a1buf[3][6]= 0.588398369;
Michu90 10:605b0bfadc2e 654 a1buf[3][7]= 0.588305183;
Michu90 10:605b0bfadc2e 655 a1buf[3][8]= 0.588305183;
Michu90 10:605b0bfadc2e 656
Michu90 10:605b0bfadc2e 657 a2buf[0][0]= -14020.22132;
Michu90 10:605b0bfadc2e 658 a2buf[0][1]= -10629.35935;
Michu90 10:605b0bfadc2e 659 a2buf[0][2]= -8667.80431;
Michu90 10:605b0bfadc2e 660 a2buf[0][3]= -7195.189284;
Michu90 10:605b0bfadc2e 661 a2buf[0][4]= -5935.445545;
Michu90 10:605b0bfadc2e 662 a2buf[0][5]= -4432.964473;
Michu90 10:605b0bfadc2e 663 a2buf[0][6]= -3899.481654;
Michu90 10:605b0bfadc2e 664 a2buf[0][7]= -3899.481654;
Michu90 10:605b0bfadc2e 665 a2buf[0][8]= -3899.481654;
Michu90 10:605b0bfadc2e 666 a2buf[1][0]= -15394.5894;
Michu90 10:605b0bfadc2e 667 a2buf[1][1]= -11261.14735;
Michu90 10:605b0bfadc2e 668 a2buf[1][2]= -9187.344205;
Michu90 10:605b0bfadc2e 669 a2buf[1][3]= -7648.375073;
Michu90 10:605b0bfadc2e 670 a2buf[1][4]= -5478.811881;
Michu90 10:605b0bfadc2e 671 a2buf[1][5]= -4780.460105;
Michu90 10:605b0bfadc2e 672 a2buf[1][6]= -4131.426907;
Michu90 10:605b0bfadc2e 673 a2buf[1][7]= -4131.426907;
Michu90 10:605b0bfadc2e 674 a2buf[1][8]= -4131.426907;
Michu90 10:605b0bfadc2e 675 a2buf[2][0]= -14446.09785;
Michu90 10:605b0bfadc2e 676 a2buf[2][1]= -10984.43215;
Michu90 10:605b0bfadc2e 677 a2buf[2][2]= -8760.145603;
Michu90 10:605b0bfadc2e 678 a2buf[2][3]= -7434.705882;
Michu90 10:605b0bfadc2e 679 a2buf[2][4]= -5971.980198;
Michu90 10:605b0bfadc2e 680 a2buf[2][5]= -4525.428072;
Michu90 10:605b0bfadc2e 681 a2buf[2][6]= -4525.428072;
Michu90 10:605b0bfadc2e 682 a2buf[2][7]= -4525.428072;
Michu90 10:605b0bfadc2e 683 a2buf[2][8]= -4525.428072;
Michu90 10:605b0bfadc2e 684 a2buf[3][0]= -13645.61444;
Michu90 10:605b0bfadc2e 685 a2buf[3][1]= -10761.08911;
Michu90 10:605b0bfadc2e 686 a2buf[3][2]= -8579.586488;
Michu90 10:605b0bfadc2e 687 a2buf[3][3]= -7342.912056;
Michu90 10:605b0bfadc2e 688 a2buf[3][4]= -5896.039604;
Michu90 10:605b0bfadc2e 689 a2buf[3][5]= -4603.83809;
Michu90 10:605b0bfadc2e 690 a2buf[3][6]= -3978.10134;
Michu90 10:605b0bfadc2e 691 a2buf[3][7]= -3976.928363;
Michu90 10:605b0bfadc2e 692 a2buf[3][8]= -3976.928363;
Michu90 10:605b0bfadc2e 693
Michu90 10:605b0bfadc2e 694 Omegamax[0][0]= 1790;
Michu90 10:605b0bfadc2e 695 Omegamax[0][1]= 2360;
Michu90 10:605b0bfadc2e 696 Omegamax[0][2]= 2830;
Michu90 10:605b0bfadc2e 697 Omegamax[0][3]= 3250;
Michu90 10:605b0bfadc2e 698 Omegamax[0][4]= 3620;
Michu90 10:605b0bfadc2e 699 Omegamax[0][5]= 3950;
Michu90 10:605b0bfadc2e 700 Omegamax[0][6]= 4230;
Michu90 10:605b0bfadc2e 701 Omegamax[0][7]= 4270;
Michu90 10:605b0bfadc2e 702 Omegamax[0][8]= 20000;
Michu90 10:605b0bfadc2e 703 Omegamax[1][0]= 1710;
Michu90 10:605b0bfadc2e 704 Omegamax[1][1]= 2300;
Michu90 10:605b0bfadc2e 705 Omegamax[1][2]= 2820;
Michu90 10:605b0bfadc2e 706 Omegamax[1][3]= 3260;
Michu90 10:605b0bfadc2e 707 Omegamax[1][4]= 3590;
Michu90 10:605b0bfadc2e 708 Omegamax[1][5]= 3920;
Michu90 10:605b0bfadc2e 709 Omegamax[1][6]= 4220;
Michu90 10:605b0bfadc2e 710 Omegamax[1][7]= 4240;
Michu90 10:605b0bfadc2e 711 Omegamax[1][8]= 20000;
Michu90 10:605b0bfadc2e 712 Omegamax[2][0]= 1760;
Michu90 10:605b0bfadc2e 713 Omegamax[2][1]= 2360;
Michu90 10:605b0bfadc2e 714 Omegamax[2][2]= 2810;
Michu90 10:605b0bfadc2e 715 Omegamax[2][3]= 3250;
Michu90 10:605b0bfadc2e 716 Omegamax[2][4]= 3630;
Michu90 10:605b0bfadc2e 717 Omegamax[2][5]= 3940;
Michu90 10:605b0bfadc2e 718 Omegamax[2][6]= 4110;
Michu90 10:605b0bfadc2e 719 Omegamax[2][7]= 4110;
Michu90 10:605b0bfadc2e 720 Omegamax[2][8]= 20000;
Michu90 10:605b0bfadc2e 721 Omegamax[3][0]= 1790;
Michu90 10:605b0bfadc2e 722 Omegamax[3][1]= 2380;
Michu90 10:605b0bfadc2e 723 Omegamax[3][2]= 2880;
Michu90 10:605b0bfadc2e 724 Omegamax[3][3]= 3270;
Michu90 10:605b0bfadc2e 725 Omegamax[3][4]= 3650;
Michu90 10:605b0bfadc2e 726 Omegamax[3][5]= 3970;
Michu90 10:605b0bfadc2e 727 Omegamax[3][6]= 4250;
Michu90 10:605b0bfadc2e 728 Omegamax[3][7]= 4300;
Michu90 10:605b0bfadc2e 729 Omegamax[3][8]= 20000;
Michu90 11:38db3ed41f13 730
Michu90 11:38db3ed41f13 731 a1[0]=a1buf[0][0];
Michu90 11:38db3ed41f13 732 a2[0]=a2buf[0][0];
Michu90 11:38db3ed41f13 733 a1[1]=a1buf[1][0];
Michu90 11:38db3ed41f13 734 a2[1]=a2buf[1][0];
Michu90 11:38db3ed41f13 735 a1[2]=a1buf[2][0];
Michu90 11:38db3ed41f13 736 a2[2]=a2buf[2][0];
Michu90 11:38db3ed41f13 737 a1[3]=a1buf[3][0];
Michu90 11:38db3ed41f13 738 a2[3]=a2buf[3][0];
Michu90 11:38db3ed41f13 739
Michu90 11:38db3ed41f13 740 triger1.attach(&task1, 0.005);
Michu90 11:38db3ed41f13 741 //triger2.attach(&task2, 0.005);
Michu90 11:38db3ed41f13 742 //triger3.attach(&task3, 0.005);
Michu90 11:38db3ed41f13 743 //tachometer.attach(&task_tachometer, 3.5);
Michu90 10:605b0bfadc2e 744
Michu90 10:605b0bfadc2e 745
Michu90 10:605b0bfadc2e 746
Igor_W 2:9e6ed6302c78 747 while(1) {
Michu90 6:8cc6df266363 748
Michu90 7:2ba30a0cdc16 749 //myled2 = !myled2;
Michu90 8:dc48ce79ad59 750 //sprintf(buff, "%f,%f,%f,0,%f,%f,%f,%f,%f,%f,\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1,valPWM3,Kp2,Kd2,Ki2,rollE);
Michu90 8:dc48ce79ad59 751 //pc.printf(buff);
Michu90 8:dc48ce79ad59 752 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI), -katy*180/M_PI, roll*180/M_PI, o[1]*180/M_PI,valPWM3);
Michu90 5:c3caf8b83e6b 753 //pc.printf(buff);
Michu90 8:dc48ce79ad59 754 //sprintf(buff, "%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, katzyro*180/M_PI, fkompl*180/M_PI,roll2*180/M_PI);
Michu90 8:dc48ce79ad59 755 //pc.printf(buff);
Michu90 8:dc48ce79ad59 756 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", d[3],d[4],d[5],xmin,xmax,ymin,ymax,zmin,zmax);
Michu90 8:dc48ce79ad59 757 //pc.printf(buff);
Michu90 8:dc48ce79ad59 758 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", d[3],filtrWynik,d[4],filtr2Wynik,d[5],filtr3Wynik,xmin,xmax,ymin,ymax,zmin,zmax);
Michu90 7:2ba30a0cdc16 759 //pc.printf(buff);
Michu90 8:dc48ce79ad59 760 //sprintf(buff, "%f,%f,%f\n\r", d[0],d[1],d[2]);
Michu90 8:dc48ce79ad59 761 //pc.printf(buff);
Michu90 8:dc48ce79ad59 762 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,\n\r", -katy*180/M_PI,roll*180/M_PI,-katybut*180/M_PI,roll2*180/M_PI,kalmanrollzyro*180/M_PI,kalmanrollzyrobut*180/M_PI);
Michu90 8:dc48ce79ad59 763 //pc.printf(buff);
Michu90 9:4e94a16ca90c 764 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", pitch*180/M_PI,pitch2*180/M_PI,o[0]*180/M_PI,valPWM2,valPWM4,Kp1,Kd1,Ki1,pitchE*180/M_PI);
Michu90 9:4e94a16ca90c 765 //pc.printf(buff);
Michu90 9:4e94a16ca90c 766 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f\n\r", katx*180/M_PI,pitch*180/M_PI,o[0]*180/M_PI,PWM2zad,valPWM2,PWM4zad,valPWM4);
Michu90 9:4e94a16ca90c 767 //pc.printf(buff);
Michu90 9:4e94a16ca90c 768 //sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", pitch*180/M_PI, -kalmanpitchzyro*180/M_PI,Kd1,Kp1,Ki1);
Michu90 9:4e94a16ca90c 769 //pc.printf(buff);
Michu90 9:4e94a16ca90c 770 //sprintf(buff, "%f,%f,%f,%f,%f\n\r", valPWM4,RPMtach,valPWM3,valPWM2,valPWM1);
Michu90 9:4e94a16ca90c 771 //pc.printf(buff);
Michu90 8:dc48ce79ad59 772 //sprintf(buff, "%f,%f,%f,%f,%f,%f\n\r", katx*180/M_PI,pitch*180/M_PI,katxzyro*180/M_PI,o[0]*180/M_PI,kalmanpitchzyro*180/M_PI,kalmanpitchdryf*180/M_PI);
Michu90 8:dc48ce79ad59 773 //pc.printf(buff);
Michu90 10:605b0bfadc2e 774 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", yaw*180/M_PI,yawOff*180/M_PI,d[6],d[7],d[8],o[2]*180/M_PI,katz*180/M_PI);
Michu90 10:605b0bfadc2e 775 //pc.printf(buff);
Michu90 7:2ba30a0cdc16 776 //myled2 = !myled2;
Michu90 11:38db3ed41f13 777 //sprintf(buff, "%i,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", Omegazad[0],PWM1zad,PWM2zad,PWM3zad,PWM4zad,a1[0],a1[1],a1[2],a1[3],a2[0],a2[1],a2[2],a2[3]);
Michu90 11:38db3ed41f13 778 //pc.printf(buff);
Michu90 6:8cc6df266363 779
Michu90 11:38db3ed41f13 780
Michu90 11:38db3ed41f13 781
Michu90 11:38db3ed41f13 782 sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", roll*180/M_PI,valPWM1,valPWM2,valPWM3,valPWM4,Kp1,Kd1,Ki1);
Michu90 11:38db3ed41f13 783 pc.printf(buff);
Michu90 5:c3caf8b83e6b 784
Michu90 4:a5b51a651db7 785
Michu90 4:a5b51a651db7 786 if(pc.readable()){
Michu90 4:a5b51a651db7 787 znak=pc.getc();
Michu90 4:a5b51a651db7 788 switch (znak){
Michu90 5:c3caf8b83e6b 789
Michu90 8:dc48ce79ad59 790 case 'r':
Michu90 8:dc48ce79ad59 791 sprintf(buff, "Resetuje zmienne %c\n\r",znak);
Michu90 8:dc48ce79ad59 792 pc.printf(buff);
Michu90 8:dc48ce79ad59 793 xmin=0;
Michu90 8:dc48ce79ad59 794 ymin=0;
Michu90 8:dc48ce79ad59 795 zmin=0;
Michu90 8:dc48ce79ad59 796 xmax=0;
Michu90 8:dc48ce79ad59 797 ymax=0;
Michu90 8:dc48ce79ad59 798 zmax=0;
Michu90 8:dc48ce79ad59 799 break;
Michu90 9:4e94a16ca90c 800
Michu90 9:4e94a16ca90c 801 case 't':
Michu90 9:4e94a16ca90c 802 PWM4zad++;
Michu90 10:605b0bfadc2e 803 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 804 break;
Michu90 9:4e94a16ca90c 805 case 'g':
Michu90 9:4e94a16ca90c 806 PWM4zad--;
Michu90 10:605b0bfadc2e 807 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 808 break;
Michu90 9:4e94a16ca90c 809 case 'y':
Michu90 9:4e94a16ca90c 810 PWM4zad+=10;
Michu90 10:605b0bfadc2e 811 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 812 break;
Michu90 9:4e94a16ca90c 813 case 'h':
Michu90 9:4e94a16ca90c 814 PWM4zad-=10;
Michu90 10:605b0bfadc2e 815 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 816 break;
Michu90 9:4e94a16ca90c 817 case 'u':
Michu90 9:4e94a16ca90c 818 PWM4zad+=100;
Michu90 10:605b0bfadc2e 819 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 820 break;
Michu90 9:4e94a16ca90c 821 case 'j':
Michu90 9:4e94a16ca90c 822 PWM4zad-=100;
Michu90 10:605b0bfadc2e 823 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 824 break;
Michu90 9:4e94a16ca90c 825 case 's':
Michu90 9:4e94a16ca90c 826 PWM4zad=10000;
Michu90 10:605b0bfadc2e 827 M3.pulsewidth_us(PWM4zad);
Michu90 9:4e94a16ca90c 828 break;
Michu90 9:4e94a16ca90c 829 case 'b':
Michu90 9:4e94a16ca90c 830 PWM4zad=10000;
Michu90 10:605b0bfadc2e 831 M3.pulsewidth_us(PWM4zad);
Michu90 10:605b0bfadc2e 832 break;
Michu90 10:605b0bfadc2e 833 case 'z':
Michu90 10:605b0bfadc2e 834 FlagaPom=1;
Michu90 9:4e94a16ca90c 835 break;
Michu90 10:605b0bfadc2e 836 case 'x':
Michu90 10:605b0bfadc2e 837 FlagaPom=0;
Michu90 10:605b0bfadc2e 838 break;
Michu90 10:605b0bfadc2e 839
Michu90 7:2ba30a0cdc16 840 /*case 'u':
Michu90 7:2ba30a0cdc16 841 PWM1zad+=10;
Michu90 7:2ba30a0cdc16 842 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 843 break;
Michu90 7:2ba30a0cdc16 844 case 'j':
Michu90 7:2ba30a0cdc16 845 PWM1zad-=10;
Michu90 7:2ba30a0cdc16 846 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 847 break;*/
Michu90 4:a5b51a651db7 848 }
Michu90 4:a5b51a651db7 849
Michu90 5:c3caf8b83e6b 850 znak=0;
Michu90 10:605b0bfadc2e 851 }
Michu90 5:c3caf8b83e6b 852
Michu90 4:a5b51a651db7 853
Michu90 4:a5b51a651db7 854 if(bluetooth.readable()){
Michu90 4:a5b51a651db7 855
Michu90 4:a5b51a651db7 856 znak2=bluetooth.getc();
Michu90 4:a5b51a651db7 857
Michu90 5:c3caf8b83e6b 858 switch (znak2){
Michu90 10:605b0bfadc2e 859
Michu90 10:605b0bfadc2e 860
Michu90 10:605b0bfadc2e 861 case 'a':
Michu90 10:605b0bfadc2e 862 for(jster=0;jster<4;jster++){
Michu90 10:605b0bfadc2e 863 Omegazad[jster]-=50;
Michu90 10:605b0bfadc2e 864 }
Michu90 10:605b0bfadc2e 865 flagaster=1;
Michu90 10:605b0bfadc2e 866
Michu90 9:4e94a16ca90c 867 /*T1zad-=0.05;
Michu90 9:4e94a16ca90c 868 T2zad-=0.05;
Michu90 9:4e94a16ca90c 869 T3zad-=0.05;
Michu90 9:4e94a16ca90c 870 T4zad-=0.05;
Michu90 9:4e94a16ca90c 871 PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
Michu90 9:4e94a16ca90c 872 PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
Michu90 9:4e94a16ca90c 873 PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
Michu90 9:4e94a16ca90c 874 PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
Michu90 9:4e94a16ca90c 875
Michu90 10:605b0bfadc2e 876 /*
Michu90 8:dc48ce79ad59 877 //PWM3zad-=50;
Michu90 9:4e94a16ca90c 878 PWM4zad-=50;
Michu90 9:4e94a16ca90c 879 //PWM2zad=1.045*(PWM4zad-10000)+10000;
Michu90 8:dc48ce79ad59 880 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 9:4e94a16ca90c 881
Michu90 9:4e94a16ca90c 882 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 883 //PWM1zad-=50;
Michu90 5:c3caf8b83e6b 884 if(PWM1zad<10000){
Michu90 5:c3caf8b83e6b 885 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 886 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 887 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 888 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 889 }
Michu90 5:c3caf8b83e6b 890 //ustawianie
Michu90 5:c3caf8b83e6b 891 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 892 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 893 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 894 M4.pulsewidth_us(PWM4zad);
Michu90 10:605b0bfadc2e 895 */
Michu90 5:c3caf8b83e6b 896 znak2=0;
Michu90 10:605b0bfadc2e 897
Michu90 4:a5b51a651db7 898 break;
Michu90 4:a5b51a651db7 899
Michu90 4:a5b51a651db7 900 case 'b':
Michu90 10:605b0bfadc2e 901 for(jster=0;jster<4;jster++){
Michu90 10:605b0bfadc2e 902 Omegazad[jster]+=50;
Michu90 10:605b0bfadc2e 903 }
Michu90 10:605b0bfadc2e 904 flagaster=1;
Michu90 9:4e94a16ca90c 905 /*T1zad+=0.05;
Michu90 9:4e94a16ca90c 906 T2zad+=0.05;
Michu90 9:4e94a16ca90c 907 T3zad+=0.05;
Michu90 9:4e94a16ca90c 908 T4zad+=0.05;
Michu90 9:4e94a16ca90c 909 PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
Michu90 9:4e94a16ca90c 910 PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
Michu90 9:4e94a16ca90c 911 PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
Michu90 9:4e94a16ca90c 912 PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
Michu90 10:605b0bfadc2e 913 /*
Michu90 8:dc48ce79ad59 914 //PWM3zad+=50;
Michu90 9:4e94a16ca90c 915 PWM4zad+=50;
Michu90 9:4e94a16ca90c 916 //PWM2zad=1.045*(PWM4zad-10000)+10000;
Michu90 9:4e94a16ca90c 917 //PWM3zad+=50;
Michu90 8:dc48ce79ad59 918 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 919
Michu90 8:dc48ce79ad59 920 //PWM1zad+=50;
Michu90 5:c3caf8b83e6b 921 if(PWM1zad>=20000){
Michu90 5:c3caf8b83e6b 922 PWM1zad=20000;
Michu90 5:c3caf8b83e6b 923 PWM2zad=20000;
Michu90 5:c3caf8b83e6b 924 PWM3zad=20000;
Michu90 5:c3caf8b83e6b 925 PWM4zad=20000;
Michu90 5:c3caf8b83e6b 926 }
Michu90 5:c3caf8b83e6b 927 //ustawianie
Michu90 5:c3caf8b83e6b 928 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 929 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 930 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 931 M4.pulsewidth_us(PWM4zad);
Michu90 10:605b0bfadc2e 932 */
Michu90 5:c3caf8b83e6b 933 znak2=0;
Michu90 5:c3caf8b83e6b 934 break;
Michu90 5:c3caf8b83e6b 935
Michu90 5:c3caf8b83e6b 936 case 'x':
Michu90 9:4e94a16ca90c 937 //sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
Michu90 9:4e94a16ca90c 938 //pc.printf(buff);
Michu90 9:4e94a16ca90c 939 //wait(1.0f);
Michu90 10:605b0bfadc2e 940 for(jster=0;jster<4;jster++){
Michu90 10:605b0bfadc2e 941 Omegazad[jster]=0;
Michu90 10:605b0bfadc2e 942 }
Michu90 5:c3caf8b83e6b 943 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 944 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 945 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 946 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 947 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 948 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 949 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 950 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 951 sprintf(buff,"Odlacz silniki\n\r");
Michu90 5:c3caf8b83e6b 952 pc.printf(buff);
Michu90 5:c3caf8b83e6b 953 wait(1.0f);
Michu90 5:c3caf8b83e6b 954 sprintf(buff,"5 \n\r");
Michu90 5:c3caf8b83e6b 955 pc.printf(buff);
Michu90 5:c3caf8b83e6b 956 wait(1.0f);
Michu90 5:c3caf8b83e6b 957 sprintf(buff,"4 \n\r");
Michu90 5:c3caf8b83e6b 958 pc.printf(buff);
Michu90 5:c3caf8b83e6b 959 wait(1.0f);
Michu90 5:c3caf8b83e6b 960 sprintf(buff,"3 \n\r");
Michu90 5:c3caf8b83e6b 961 pc.printf(buff);
Michu90 5:c3caf8b83e6b 962 wait(1.0f);
Michu90 5:c3caf8b83e6b 963 sprintf(buff,"2 \n\r");
Michu90 5:c3caf8b83e6b 964 pc.printf(buff);
Michu90 5:c3caf8b83e6b 965 wait(1.0f);
Michu90 5:c3caf8b83e6b 966 sprintf(buff,"1 \n\r");
Michu90 5:c3caf8b83e6b 967 pc.printf(buff);
Michu90 5:c3caf8b83e6b 968 wait(1.0f);
Michu90 5:c3caf8b83e6b 969 sprintf(buff,"GO! \n\r");
Michu90 4:a5b51a651db7 970 pc.printf(buff);
Michu90 4:a5b51a651db7 971 break;
Michu90 4:a5b51a651db7 972
Michu90 6:8cc6df266363 973 case 'c':
Michu90 8:dc48ce79ad59 974 Kd1-=0.5;
Michu90 8:dc48ce79ad59 975 Kd2-=0.5;
Michu90 6:8cc6df266363 976 break;
Michu90 6:8cc6df266363 977 case 'd':
Michu90 8:dc48ce79ad59 978 Kd1+=0.5;
Michu90 8:dc48ce79ad59 979 Kd2+=0.5;
Michu90 6:8cc6df266363 980 break;
Michu90 6:8cc6df266363 981 case 'e':
Michu90 6:8cc6df266363 982 Kp1-=0.5;
Michu90 6:8cc6df266363 983 Kp2-=0.5;
Michu90 6:8cc6df266363 984 break;
Michu90 6:8cc6df266363 985 case 'f':
Michu90 6:8cc6df266363 986 Kp1+=0.5;
Michu90 6:8cc6df266363 987 Kp2+=0.5;
Michu90 8:dc48ce79ad59 988 break;
Michu90 8:dc48ce79ad59 989 case 'g':
Michu90 8:dc48ce79ad59 990 Ki1-=0.1;
Michu90 8:dc48ce79ad59 991 Ki2-=0.1;
Michu90 8:dc48ce79ad59 992 break;
Michu90 8:dc48ce79ad59 993 case 'h':
Michu90 8:dc48ce79ad59 994 Ki1+=0.1;
Michu90 8:dc48ce79ad59 995 Ki2+=0.1;
Michu90 8:dc48ce79ad59 996 break;
Michu90 10:605b0bfadc2e 997 }
Michu90 10:605b0bfadc2e 998
Michu90 10:605b0bfadc2e 999
Michu90 10:605b0bfadc2e 1000 if(flagaster==1){
Michu90 10:605b0bfadc2e 1001
Michu90 10:605b0bfadc2e 1002 for(int jster=0;jster<4;jster++){
Michu90 10:605b0bfadc2e 1003 ister=-1;
Michu90 10:605b0bfadc2e 1004 do{
Michu90 10:605b0bfadc2e 1005 ister++;
Michu90 10:605b0bfadc2e 1006 }while(Omegazad[jster] > Omegamax[jster][ister]);
Michu90 10:605b0bfadc2e 1007 a1[jster]=a1buf[jster][ister];
Michu90 10:605b0bfadc2e 1008 a2[jster]=a2buf[jster][ister];
Michu90 10:605b0bfadc2e 1009 }
Michu90 10:605b0bfadc2e 1010
Michu90 10:605b0bfadc2e 1011 PWM1zad=(Omegazad[0]-a2[0])/a1[0];
Michu90 10:605b0bfadc2e 1012 PWM2zad=(Omegazad[1]-a2[1])/a1[1];
Michu90 10:605b0bfadc2e 1013 PWM3zad=(Omegazad[2]-a2[2])/a1[2];
Michu90 10:605b0bfadc2e 1014 PWM4zad=(Omegazad[3]-a2[3])/a1[3];
Michu90 10:605b0bfadc2e 1015
Michu90 10:605b0bfadc2e 1016 if(PWM1zad<10000){
Michu90 10:605b0bfadc2e 1017 PWM1zad=10000;
Michu90 10:605b0bfadc2e 1018 }
Michu90 10:605b0bfadc2e 1019 if(PWM2zad<10000){
Michu90 10:605b0bfadc2e 1020 PWM2zad=10000;
Michu90 10:605b0bfadc2e 1021 }
Michu90 10:605b0bfadc2e 1022 if(PWM3zad<10000){
Michu90 10:605b0bfadc2e 1023 PWM3zad=10000;
Michu90 10:605b0bfadc2e 1024 }
Michu90 10:605b0bfadc2e 1025 if(PWM4zad<10000){
Michu90 10:605b0bfadc2e 1026 PWM4zad=10000;
Michu90 10:605b0bfadc2e 1027 }
Michu90 10:605b0bfadc2e 1028 M1.pulsewidth_us(PWM1zad);
Michu90 10:605b0bfadc2e 1029 M2.pulsewidth_us(PWM2zad);
Michu90 10:605b0bfadc2e 1030 M3.pulsewidth_us(PWM3zad);
Michu90 10:605b0bfadc2e 1031 M4.pulsewidth_us(PWM4zad);
Michu90 10:605b0bfadc2e 1032
Michu90 10:605b0bfadc2e 1033 flagaster=0;
Michu90 4:a5b51a651db7 1034 }
Michu90 10:605b0bfadc2e 1035
Michu90 10:605b0bfadc2e 1036 }//switch
Michu90 6:8cc6df266363 1037 //myled2 = !myled2;
Michu90 10:605b0bfadc2e 1038 }//while(1)
Michu90 10:605b0bfadc2e 1039 }//main