Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@10:605b0bfadc2e, 2015-02-10 (annotated)

Committer:
Michu90
Date:
Tue Feb 10 15:08:04 2015 +0000
Revision:
10:605b0bfadc2e
Parent:
9:4e94a16ca90c
Child:
11:38db3ed41f13
Jeszcze sterowanie do zmiany jest tylko

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