Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@8:dc48ce79ad59, 2015-01-21 (annotated)

Committer:
Michu90
Date:
Wed Jan 21 16:30:33 2015 +0000
Revision:
8:dc48ce79ad59
Parent:
7:2ba30a0cdc16
Child:
9:4e94a16ca90c
2015_01_21

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 8:dc48ce79ad59 12 #define filtrWymiar 100
Michu90 8:dc48ce79ad59 13 #define filtrSygnal d[3]
Michu90 8:dc48ce79ad59 14 #define filtr2Wymiar 100
Michu90 8:dc48ce79ad59 15 #define filtr2Sygnal d[4]
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 5:c3caf8b83e6b 34
Michu90 5:c3caf8b83e6b 35 FastPWM M1(D10);
Michu90 5:c3caf8b83e6b 36 FastPWM M2(D11);
Michu90 5:c3caf8b83e6b 37 FastPWM M3(D12);
Michu90 5:c3caf8b83e6b 38 FastPWM M4(D13);
Igor_W 2:9e6ed6302c78 39
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
Igor_W 2:9e6ed6302c78 44
Michu90 5:c3caf8b83e6b 45 float d[9];
Michu90 5:c3caf8b83e6b 46 double D[9];
Michu90 5:c3caf8b83e6b 47 float o[3];
Michu90 5:c3caf8b83e6b 48 float O[3];
Michu90 5:c3caf8b83e6b 49 char buff[160];
Michu90 5:c3caf8b83e6b 50 float r,katx,katy;
Michu90 5:c3caf8b83e6b 51 float rbut,katxbut,katybut;
Michu90 5:c3caf8b83e6b 52 float pitch, roll;
Michu90 5:c3caf8b83e6b 53 float pitch2, roll2;
Michu90 7:2ba30a0cdc16 54 float pitchE, rollE;
Michu90 5:c3caf8b83e6b 55 double i;
Michu90 5:c3caf8b83e6b 56 float offsetGyro[3];
Michu90 5:c3caf8b83e6b 57 char odczyt[20];
Michu90 4:a5b51a651db7 58 char znak;
Michu90 4:a5b51a651db7 59 char znak2;
Michu90 7:2ba30a0cdc16 60 float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
Michu90 6:8cc6df266363 61 float U1,U2,U3;
Michu90 6:8cc6df266363 62 float Om1,Om2,Om3,Om4;
Michu90 6:8cc6df266363 63 float wyp1,wyp2,wyp3,wyp4;
Michu90 5:c3caf8b83e6b 64
Michu90 5:c3caf8b83e6b 65 double PWM1zad;
Michu90 5:c3caf8b83e6b 66 double PWM2zad;
Michu90 5:c3caf8b83e6b 67 double PWM3zad;
Michu90 5:c3caf8b83e6b 68 double PWM4zad;
Michu90 5:c3caf8b83e6b 69 double valPWM1;
Michu90 5:c3caf8b83e6b 70 double valPWM2;
Michu90 5:c3caf8b83e6b 71 double valPWM3;
Michu90 5:c3caf8b83e6b 72 double valPWM4;
Michu90 8:dc48ce79ad59 73 float T1zad,T2zad,T3zad,T4zad;
Michu90 8:dc48ce79ad59 74
Michu90 8:dc48ce79ad59 75 //zmienne tymczasowe
Michu90 8:dc48ce79ad59 76 float katxzyro,katyzyro,katzzyro;
Michu90 8:dc48ce79ad59 77 float fkompl;
Michu90 8:dc48ce79ad59 78 float xmin, ymin, zmin;
Michu90 8:dc48ce79ad59 79 float xmax, ymax, zmax;
Michu90 8:dc48ce79ad59 80 float filtrBufor[filtrWymiar];
Michu90 8:dc48ce79ad59 81 int filtrZmienna=0;
Michu90 8:dc48ce79ad59 82 float filtrSuma=0;
Michu90 8:dc48ce79ad59 83 float filtrWynik;
Michu90 8:dc48ce79ad59 84 float filtr2Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 85 int filtr2Zmienna=0;
Michu90 8:dc48ce79ad59 86 float filtr2Suma=0;
Michu90 8:dc48ce79ad59 87 float filtr2Wynik;
Michu90 8:dc48ce79ad59 88 float filtr3Bufor[filtrWymiar];
Michu90 8:dc48ce79ad59 89 int filtr3Zmienna=0;
Michu90 8:dc48ce79ad59 90 float filtr3Suma=0;
Michu90 8:dc48ce79ad59 91 float filtr3Wynik;
Michu90 8:dc48ce79ad59 92
Michu90 8:dc48ce79ad59 93 float kalmanpitchzyro;
Michu90 8:dc48ce79ad59 94 float kalmanrollzyro;
Michu90 8:dc48ce79ad59 95 float kalmanrollzyrobut;
Michu90 8:dc48ce79ad59 96 float kalmanpitchdryf;
Michu90 5:c3caf8b83e6b 97
Michu90 5:c3caf8b83e6b 98 void task1()
Michu90 5:c3caf8b83e6b 99 {
Michu90 7:2ba30a0cdc16 100 //myled = !myled;
Michu90 5:c3caf8b83e6b 101 imu.readData(d);
Michu90 5:c3caf8b83e6b 102 imu.filterData(d, D);
Michu90 5:c3caf8b83e6b 103 off.offsetData(d,offsetGyro,o);
Michu90 5:c3caf8b83e6b 104 off.offsetData2(D,offsetGyro,O);
Michu90 5:c3caf8b83e6b 105
Michu90 5:c3caf8b83e6b 106 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 5:c3caf8b83e6b 107 katx = acos(d[4]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 108 katy = acos(d[3]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 109
Michu90 5:c3caf8b83e6b 110 rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
Michu90 5:c3caf8b83e6b 111 katxbut = acos(D[4]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 112 katybut = acos(D[3]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 113
Michu90 8:dc48ce79ad59 114 //katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 115 //fkompl = katyzyro*0.95+ (-katy)*0.05;
Michu90 5:c3caf8b83e6b 116
Michu90 5:c3caf8b83e6b 117 //Filtr Kalmana
Michu90 5:c3caf8b83e6b 118 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 5:c3caf8b83e6b 119 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 5:c3caf8b83e6b 120 pitch = pitch_data.x1;
Michu90 5:c3caf8b83e6b 121 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 122 kalmanrollzyro = roll_data.x2;
Michu90 5:c3caf8b83e6b 123
Michu90 5:c3caf8b83e6b 124 //Filtr Kalmana butterworth 2nd
Michu90 5:c3caf8b83e6b 125 kalman_innovate(&pitch_data2, katxbut, O[0]);
Michu90 5:c3caf8b83e6b 126 kalman_innovate(&roll_data2, -katybut, O[1]);
Michu90 5:c3caf8b83e6b 127 pitch2 = pitch_data2.x1;
Michu90 5:c3caf8b83e6b 128 roll2 = roll_data2.x1;
Michu90 8:dc48ce79ad59 129 kalmanrollzyrobut = roll_data2.x2;
Michu90 5:c3caf8b83e6b 130
Michu90 7:2ba30a0cdc16 131 //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
Michu90 7:2ba30a0cdc16 132 //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
Michu90 7:2ba30a0cdc16 133 //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
Michu90 6:8cc6df266363 134
Michu90 7:2ba30a0cdc16 135 //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 136 //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 137 //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 138 //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
Michu90 6:8cc6df266363 139
Michu90 7:2ba30a0cdc16 140 /*wyp1 = sqrt(Om1)*13.17523+10000;
Michu90 6:8cc6df266363 141 wyp2 = sqrt(Om2)*13.17523+10000;
Michu90 6:8cc6df266363 142 wyp3 = sqrt(Om3)*13.17523+10000;
Michu90 7:2ba30a0cdc16 143 wyp4 = sqrt(Om4)*13.17523+10000;*/
Michu90 6:8cc6df266363 144
Michu90 6:8cc6df266363 145
Michu90 7:2ba30a0cdc16 146 // *********************** DISCRETE PID CONTROLLER **********************
Michu90 7:2ba30a0cdc16 147 // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
Michu90 7:2ba30a0cdc16 148 // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
Michu90 7:2ba30a0cdc16 149 // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
Michu90 7:2ba30a0cdc16 150 //omega1^2=(B1/b*PWM1zad+C1/b)-U2/2bl+U3/4d
Michu90 7:2ba30a0cdc16 151 //omega2^2=(B/b*PWM2zad+C/b)+U1/2bl-U3/4d
Michu90 7:2ba30a0cdc16 152 //omega3^2=(B3/b*PWM3zad+C3/b)+U2/2bl+U3/4d
Michu90 7:2ba30a0cdc16 153 //omega4^2=(B/b*PWM4zad+C/b)-U1/2bl-U3/4d
Michu90 7:2ba30a0cdc16 154 //wyp1=b/B1*omega1^2-C1/B1
Michu90 7:2ba30a0cdc16 155 //wyp2=b/B*omega2^2-C/B
Michu90 7:2ba30a0cdc16 156 //wyp3=b/B3*omega3^2-C3/B3
Michu90 7:2ba30a0cdc16 157 //wyp4=b/B*omega4^2-C/B
Michu90 7:2ba30a0cdc16 158
Michu90 7:2ba30a0cdc16 159 //b=0.000015
Michu90 7:2ba30a0cdc16 160 //B1=0.000776646
Michu90 7:2ba30a0cdc16 161 //C1=-0.334958973
Michu90 7:2ba30a0cdc16 162 //l=0.3
Michu90 7:2ba30a0cdc16 163 //d=0.000033
Michu90 7:2ba30a0cdc16 164 //2bl=0.0000092780
Michu90 7:2ba30a0cdc16 165 //4d=0.000132
Michu90 7:2ba30a0cdc16 166 //B1/b=50.22498189
Michu90 7:2ba30a0cdc16 167 //C1/b=-21611.4954
Michu90 7:2ba30a0cdc16 168 //b/B1=0.01991041
Michu90 7:2ba30a0cdc16 169 //C1/B1=-431.2892625
Michu90 8:dc48ce79ad59 170 //1/B1=1287.588322
Michu90 8:dc48ce79ad59 171
Michu90 7:2ba30a0cdc16 172
Michu90 7:2ba30a0cdc16 173 //B3/b=49.90897978
Michu90 7:2ba30a0cdc16 174 //C3/b=-25683.36221
Michu90 7:2ba30a0cdc16 175 //b/B3=0.020036474
Michu90 7:2ba30a0cdc16 176 //C3/B3=-514.6040317
Michu90 8:dc48ce79ad59 177
Michu90 8:dc48ce79ad59 178 //B*PWMzad+C=T
Michu90 8:dc48ce79ad59 179 //PWMzad=1/B*T-C/B
Michu90 8:dc48ce79ad59 180 /*
Michu90 8:dc48ce79ad59 181 PWMzad1=1287.588322*T1zad-(-431.2892625);
Michu90 8:dc48ce79ad59 182 PWMzad2=1287.588322*T2zad-(-431.2892625);
Michu90 8:dc48ce79ad59 183 PWMzad3=1287.588322*T3zad-(-431.2892625);
Michu90 8:dc48ce79ad59 184 PWMzad4=1287.588322*T4zad-(-431.2892625);*/
Michu90 7:2ba30a0cdc16 185
Michu90 7:2ba30a0cdc16 186 pitchE=pitchE+(0-pitch2);
Michu90 7:2ba30a0cdc16 187 if(pitchE>3) pitchE=3;
Michu90 8:dc48ce79ad59 188 //Kd1=Kp1*Td1/T;
Michu90 8:dc48ce79ad59 189 //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
Michu90 6:8cc6df266363 190
Michu90 7:2ba30a0cdc16 191 rollE=rollE+(0-roll2);
Michu90 8:dc48ce79ad59 192 if(rollE>5) rollE=5;
Michu90 8:dc48ce79ad59 193 if(rollE<-5) rollE=-5;
Michu90 8:dc48ce79ad59 194 //Kd2=Kp2*Td2/T;
Michu90 8:dc48ce79ad59 195 //if(Ti2==0){Ki2=0;}else Ki2=Kp2*T/Ti2;
Michu90 7:2ba30a0cdc16 196
Michu90 7:2ba30a0cdc16 197 /* yawE=yawE+(0-roll2);
Michu90 7:2ba30a0cdc16 198 if(yawE>3) yawE=3;*/
Michu90 7:2ba30a0cdc16 199 Kd3=Kp3*Td3/T;
Michu90 8:dc48ce79ad59 200 //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
Michu90 7:2ba30a0cdc16 201
Michu90 7:2ba30a0cdc16 202 U1 = 0.0173*(Kp1*(0-pitch2)+Kd1*(0-O[0])+Ki1*pitchE);
Michu90 8:dc48ce79ad59 203 U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
Michu90 7:2ba30a0cdc16 204 U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
Michu90 7:2ba30a0cdc16 205
Michu90 7:2ba30a0cdc16 206 Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 207 Om2 = 50.22498189*PWM2zad+(-21611.4954)+U1/0.0000092780 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 208 Om3 = 49.90897978*PWM3zad+(-25683.36221)+U2/0.0000092780 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 209 Om4 = 50.22498189*PWM4zad+(-21611.4954)-U1/0.0000092780 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 210
Michu90 8:dc48ce79ad59 211 wyp1=0.01991041*Om1-(-431.2892625);
Michu90 8:dc48ce79ad59 212 wyp2=0.01991041*Om2-(-431.2892625);
Michu90 8:dc48ce79ad59 213 wyp3=0.020036474*Om3-(-514.6040317);
Michu90 8:dc48ce79ad59 214 wyp4=0.01991041*Om4-(-431.2892625);
Michu90 7:2ba30a0cdc16 215
Michu90 6:8cc6df266363 216 if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
Michu90 6:8cc6df266363 217 if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
Michu90 6:8cc6df266363 218 if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
Michu90 6:8cc6df266363 219
Michu90 6:8cc6df266363 220 if(wyp2<=10001 & wyp2>40001) valPWM2=10000;
Michu90 6:8cc6df266363 221 if(wyp2>=20000 & wyp2<40000) valPWM2=20000;
Michu90 6:8cc6df266363 222 if(wyp2>10000 & wyp2<20000) valPWM2=(int)wyp2;
Michu90 6:8cc6df266363 223
Michu90 6:8cc6df266363 224 if(wyp3<=10001 | wyp3>40001) valPWM3=10000;
Michu90 6:8cc6df266363 225 if(wyp3>=20000 & wyp3<40000) valPWM3=20000;
Michu90 6:8cc6df266363 226 if(wyp3>10100 & wyp3<20000) valPWM3=(int)wyp3;
Michu90 6:8cc6df266363 227
Michu90 6:8cc6df266363 228 if(wyp4<=10001 & wyp4>40001) valPWM4=10000;
Michu90 6:8cc6df266363 229 if(wyp4>=20000 & wyp4<40000) valPWM4=20000;
Michu90 6:8cc6df266363 230 if(wyp4>10000 & wyp4<20000) valPWM4=(int)wyp4;
Michu90 6:8cc6df266363 231
Michu90 5:c3caf8b83e6b 232
Michu90 5:c3caf8b83e6b 233 //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 234 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 235 //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 236 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 237 //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 238 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 239 //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 240 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 241
Michu90 6:8cc6df266363 242 M1.pulsewidth_us(valPWM1);
Michu90 6:8cc6df266363 243 M2.pulsewidth_us(valPWM2);
Michu90 6:8cc6df266363 244 M3.pulsewidth_us(valPWM3);
Michu90 6:8cc6df266363 245 M4.pulsewidth_us(valPWM4);
Michu90 6:8cc6df266363 246
Michu90 6:8cc6df266363 247
Michu90 7:2ba30a0cdc16 248 //myled = !myled;
Michu90 5:c3caf8b83e6b 249 }
Michu90 5:c3caf8b83e6b 250
Michu90 5:c3caf8b83e6b 251
Michu90 5:c3caf8b83e6b 252 void task2()
Michu90 5:c3caf8b83e6b 253 {
Michu90 5:c3caf8b83e6b 254 /*
Michu90 5:c3caf8b83e6b 255 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 256 pc.printf(buff);
Michu90 5:c3caf8b83e6b 257 myled2 = !myled2;*/
Michu90 8:dc48ce79ad59 258 imu.readData(d);
Michu90 8:dc48ce79ad59 259
Michu90 8:dc48ce79ad59 260 if (filtrZmienna < 0) {
Michu90 8:dc48ce79ad59 261 filtrWynik = filtrSygnal;
Michu90 8:dc48ce79ad59 262 filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
Michu90 8:dc48ce79ad59 263 filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
Michu90 8:dc48ce79ad59 264 } else {
Michu90 8:dc48ce79ad59 265 if (filtrZmienna == filtrWymiar - 1) {
Michu90 8:dc48ce79ad59 266 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 267 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 268 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 269 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 270 } else {
Michu90 8:dc48ce79ad59 271 filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 272 filtrBufor[filtrZmienna] = filtrSygnal;
Michu90 8:dc48ce79ad59 273 filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
Michu90 8:dc48ce79ad59 274 filtrWynik = filtrSuma / filtrWymiar;
Michu90 8:dc48ce79ad59 275 }
Michu90 8:dc48ce79ad59 276 }
Michu90 8:dc48ce79ad59 277 filtrZmienna++;
Michu90 8:dc48ce79ad59 278 if (filtrZmienna == filtrWymiar) {
Michu90 8:dc48ce79ad59 279 filtrZmienna = 0;
Michu90 8:dc48ce79ad59 280 }
Michu90 8:dc48ce79ad59 281
Michu90 8:dc48ce79ad59 282 if (filtrWynik < xmin) xmin=filtrWynik;
Michu90 8:dc48ce79ad59 283 if (filtrWynik > xmax) xmax=filtrWynik;
Michu90 8:dc48ce79ad59 284
Michu90 8:dc48ce79ad59 285
Michu90 8:dc48ce79ad59 286 if (filtr2Zmienna < 0) {
Michu90 8:dc48ce79ad59 287 filtr2Wynik = filtr2Sygnal;
Michu90 8:dc48ce79ad59 288 filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 289 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
Michu90 8:dc48ce79ad59 290 } else {
Michu90 8:dc48ce79ad59 291 if (filtr2Zmienna == filtr2Wymiar - 1) {
Michu90 8:dc48ce79ad59 292 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 293 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 294 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 295 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 296 } else {
Michu90 8:dc48ce79ad59 297 filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 298 filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
Michu90 8:dc48ce79ad59 299 filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
Michu90 8:dc48ce79ad59 300 filtr2Wynik = filtr2Suma / filtr2Wymiar;
Michu90 8:dc48ce79ad59 301 }
Michu90 8:dc48ce79ad59 302 }
Michu90 8:dc48ce79ad59 303 filtr2Zmienna++;
Michu90 8:dc48ce79ad59 304 if (filtr2Zmienna == filtr2Wymiar) {
Michu90 8:dc48ce79ad59 305 filtr2Zmienna = 0;
Michu90 8:dc48ce79ad59 306 }
Michu90 8:dc48ce79ad59 307
Michu90 8:dc48ce79ad59 308 if (filtr2Wynik < ymin) ymin=filtr2Wynik;
Michu90 8:dc48ce79ad59 309 if (filtr2Wynik > ymax) ymax=filtr2Wynik;
Michu90 8:dc48ce79ad59 310
Michu90 8:dc48ce79ad59 311 if (filtr3Zmienna < 0) {
Michu90 8:dc48ce79ad59 312 filtr3Wynik = filtr3Sygnal;
Michu90 8:dc48ce79ad59 313 filtr3Bufor[filtr3Zmienna + filtr3Wymiar] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 314 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna + filtr3Wymiar];
Michu90 8:dc48ce79ad59 315 } else {
Michu90 8:dc48ce79ad59 316 if (filtr3Zmienna == filtr3Wymiar - 1) {
Michu90 8:dc48ce79ad59 317 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 318 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 319 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 320 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 321 } else {
Michu90 8:dc48ce79ad59 322 filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 323 filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
Michu90 8:dc48ce79ad59 324 filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
Michu90 8:dc48ce79ad59 325 filtr3Wynik = filtr3Suma / filtr3Wymiar;
Michu90 8:dc48ce79ad59 326 }
Michu90 8:dc48ce79ad59 327 }
Michu90 8:dc48ce79ad59 328 filtr3Zmienna++;
Michu90 8:dc48ce79ad59 329 if (filtr3Zmienna == filtr3Wymiar) {
Michu90 8:dc48ce79ad59 330 filtr3Zmienna = 0;
Michu90 8:dc48ce79ad59 331 }
Michu90 8:dc48ce79ad59 332
Michu90 8:dc48ce79ad59 333 if (filtr3Wynik < zmin) zmin=filtr3Wynik;
Michu90 8:dc48ce79ad59 334 if (filtr3Wynik > zmax) zmax=filtr3Wynik;
Michu90 8:dc48ce79ad59 335
Michu90 8:dc48ce79ad59 336 //sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
Michu90 8:dc48ce79ad59 337 //pc.printf(buff);
Michu90 8:dc48ce79ad59 338
Michu90 8:dc48ce79ad59 339
Michu90 5:c3caf8b83e6b 340 }
Michu90 5:c3caf8b83e6b 341
Michu90 8:dc48ce79ad59 342
Michu90 8:dc48ce79ad59 343 void task3()
Michu90 8:dc48ce79ad59 344 {
Michu90 8:dc48ce79ad59 345 imu.readData(d);
Michu90 8:dc48ce79ad59 346 off.offsetData(d,offsetGyro,o);
Michu90 8:dc48ce79ad59 347 katxzyro = katxzyro + o[0]*dt;
Michu90 8:dc48ce79ad59 348 katyzyro = katyzyro + o[1]*dt;
Michu90 8:dc48ce79ad59 349 katzzyro = katzzyro + o[2]*dt;
Michu90 8:dc48ce79ad59 350
Michu90 8:dc48ce79ad59 351 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 8:dc48ce79ad59 352 katx = acos(d[4]/r)-M_PI2;
Michu90 8:dc48ce79ad59 353 katy = acos(d[3]/r)-M_PI2;
Michu90 8:dc48ce79ad59 354
Michu90 8:dc48ce79ad59 355 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 8:dc48ce79ad59 356 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 8:dc48ce79ad59 357 pitch = pitch_data.x1;
Michu90 8:dc48ce79ad59 358 kalmanpitchzyro = pitch_data.x2;
Michu90 8:dc48ce79ad59 359 kalmanpitchdryf = pitch_data.x3;
Michu90 8:dc48ce79ad59 360 roll = roll_data.x1;
Michu90 8:dc48ce79ad59 361
Michu90 8:dc48ce79ad59 362 }
Michu90 5:c3caf8b83e6b 363
Michu90 5:c3caf8b83e6b 364
Michu90 4:a5b51a651db7 365
mbed_official 0:50d2b9c62765 366 int main() {
Igor_W 2:9e6ed6302c78 367
Michu90 4:a5b51a651db7 368 pc.baud(115200);
Michu90 4:a5b51a651db7 369 bluetooth.baud(19200);
Michu90 5:c3caf8b83e6b 370 imu.init();
Michu90 5:c3caf8b83e6b 371 kalman_init(&pitch_data);
Michu90 5:c3caf8b83e6b 372 kalman_init(&roll_data);
Michu90 5:c3caf8b83e6b 373 kalman_init(&pitch_data2);
Michu90 5:c3caf8b83e6b 374 kalman_init(&roll_data2);
Igor_W 3:1425359662e4 375
Michu90 4:a5b51a651db7 376 sprintf(buff, "Hello: \n\r");
Michu90 4:a5b51a651db7 377 pc.printf(buff);
Michu90 4:a5b51a651db7 378
Michu90 6:8cc6df266363 379 off.setOffsets(offsetGyro, pc, imu);
Michu90 5:c3caf8b83e6b 380
Michu90 5:c3caf8b83e6b 381
Michu90 5:c3caf8b83e6b 382
Michu90 5:c3caf8b83e6b 383 triger1.attach(&task1, 0.005);
Michu90 5:c3caf8b83e6b 384 //triger2.attach(&task2, 0.005);
Michu90 8:dc48ce79ad59 385 //triger3.attach(&task3, 0.005);
Michu90 5:c3caf8b83e6b 386 i=1000;
Michu90 5:c3caf8b83e6b 387
Igor_W 3:1425359662e4 388
Michu90 5:c3caf8b83e6b 389 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 390 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 391 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 392 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 393
Michu90 5:c3caf8b83e6b 394 M1.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 395 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 396 M2.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 397 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 398 M3.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 399 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 400 M4.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 401 M4.pulsewidth_us(PWM4zad);
Michu90 4:a5b51a651db7 402
Michu90 6:8cc6df266363 403 Kp1=0;
Michu90 6:8cc6df266363 404 Kp2=0;
Michu90 6:8cc6df266363 405 Kp3=0;
Michu90 8:dc48ce79ad59 406 Kd1=0;
Michu90 8:dc48ce79ad59 407 Kd2=0;
Michu90 8:dc48ce79ad59 408 Kd3=0;
Michu90 8:dc48ce79ad59 409 Ki1=0;
Michu90 8:dc48ce79ad59 410 Ki2=0;
Michu90 8:dc48ce79ad59 411 Ki3=0;
Michu90 7:2ba30a0cdc16 412 Td1=0;
Michu90 7:2ba30a0cdc16 413 Td2=0;
Michu90 7:2ba30a0cdc16 414 Td3=0;
Michu90 7:2ba30a0cdc16 415 Ti1=0;
Michu90 7:2ba30a0cdc16 416 Ti2=0;
Michu90 7:2ba30a0cdc16 417 Ti3=0;
Michu90 7:2ba30a0cdc16 418 T=0.005;
Michu90 6:8cc6df266363 419
Michu90 8:dc48ce79ad59 420 rollE=0;
Michu90 8:dc48ce79ad59 421
Michu90 8:dc48ce79ad59 422 katxzyro = 0;
Michu90 8:dc48ce79ad59 423 katyzyro = 0;
Michu90 8:dc48ce79ad59 424 katzzyro = 0;
Michu90 6:8cc6df266363 425
Igor_W 2:9e6ed6302c78 426 while(1) {
Michu90 6:8cc6df266363 427
Michu90 7:2ba30a0cdc16 428 //myled2 = !myled2;
Michu90 8:dc48ce79ad59 429
Michu90 8:dc48ce79ad59 430 //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 431 //pc.printf(buff);
Michu90 8:dc48ce79ad59 432 //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 433 //pc.printf(buff);
Michu90 8:dc48ce79ad59 434 //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 435 //pc.printf(buff);
Michu90 8:dc48ce79ad59 436 //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 437 //pc.printf(buff);
Michu90 8:dc48ce79ad59 438
Michu90 8:dc48ce79ad59 439 //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 440 //pc.printf(buff);
Michu90 8:dc48ce79ad59 441
Michu90 8:dc48ce79ad59 442 //sprintf(buff, "%f,%f,%f\n\r", d[0],d[1],d[2]);
Michu90 8:dc48ce79ad59 443 //pc.printf(buff);
Michu90 8:dc48ce79ad59 444
Michu90 8:dc48ce79ad59 445
Michu90 8:dc48ce79ad59 446 //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 447 //pc.printf(buff);
Michu90 8:dc48ce79ad59 448 sprintf(buff, "0,%f,%f,%f,%f,%f,%f,%f,%f\n\r", roll*180/M_PI,kalmanrollzyro*180/M_PI,valPWM1,valPWM3,Kp2,Kd2,Ki2,rollE*180/M_PI);
Michu90 8:dc48ce79ad59 449 pc.printf(buff);
Michu90 8:dc48ce79ad59 450
Michu90 8:dc48ce79ad59 451 //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 452 //pc.printf(buff);
Michu90 6:8cc6df266363 453
Michu90 7:2ba30a0cdc16 454 //myled2 = !myled2;
Michu90 6:8cc6df266363 455
Michu90 5:c3caf8b83e6b 456
Michu90 4:a5b51a651db7 457
Michu90 4:a5b51a651db7 458 if(pc.readable()){
Michu90 4:a5b51a651db7 459 znak=pc.getc();
Michu90 4:a5b51a651db7 460 switch (znak){
Michu90 5:c3caf8b83e6b 461
Michu90 4:a5b51a651db7 462 case 'p':
Michu90 4:a5b51a651db7 463 sprintf(buff, "odczytany znak: %c\n\r",znak);
Michu90 4:a5b51a651db7 464 pc.printf(buff);
Michu90 4:a5b51a651db7 465 break;
Michu90 8:dc48ce79ad59 466 case 'r':
Michu90 8:dc48ce79ad59 467 sprintf(buff, "Resetuje zmienne %c\n\r",znak);
Michu90 8:dc48ce79ad59 468 pc.printf(buff);
Michu90 8:dc48ce79ad59 469 xmin=0;
Michu90 8:dc48ce79ad59 470 ymin=0;
Michu90 8:dc48ce79ad59 471 zmin=0;
Michu90 8:dc48ce79ad59 472 xmax=0;
Michu90 8:dc48ce79ad59 473 ymax=0;
Michu90 8:dc48ce79ad59 474 zmax=0;
Michu90 8:dc48ce79ad59 475 break;
Michu90 7:2ba30a0cdc16 476 /*case 'u':
Michu90 7:2ba30a0cdc16 477 PWM1zad+=10;
Michu90 7:2ba30a0cdc16 478 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 479 break;
Michu90 7:2ba30a0cdc16 480 case 'j':
Michu90 7:2ba30a0cdc16 481 PWM1zad-=10;
Michu90 7:2ba30a0cdc16 482 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 483 break;*/
Michu90 4:a5b51a651db7 484
Michu90 4:a5b51a651db7 485 }
Michu90 4:a5b51a651db7 486
Michu90 5:c3caf8b83e6b 487 znak=0;
Michu90 4:a5b51a651db7 488 }
Michu90 5:c3caf8b83e6b 489
Michu90 4:a5b51a651db7 490
Michu90 4:a5b51a651db7 491 if(bluetooth.readable()){
Michu90 4:a5b51a651db7 492
Michu90 4:a5b51a651db7 493 znak2=bluetooth.getc();
Michu90 4:a5b51a651db7 494
Michu90 5:c3caf8b83e6b 495 switch (znak2){
Michu90 4:a5b51a651db7 496 case 'a':
Michu90 5:c3caf8b83e6b 497 PWM3zad-=50;
Michu90 8:dc48ce79ad59 498 //PWM2zad-=50;
Michu90 8:dc48ce79ad59 499 //PWM3zad-=50;
Michu90 8:dc48ce79ad59 500 //PWM1zad-=50;
Michu90 8:dc48ce79ad59 501 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 502 PWM1zad=1.035*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 503 //PWM1zad-=50;
Michu90 5:c3caf8b83e6b 504 if(PWM1zad<10000){
Michu90 5:c3caf8b83e6b 505 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 506 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 507 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 508 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 509 }
Michu90 5:c3caf8b83e6b 510 //ustawianie
Michu90 5:c3caf8b83e6b 511 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 512 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 513 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 514 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 515 znak2=0;
Michu90 4:a5b51a651db7 516 break;
Michu90 4:a5b51a651db7 517
Michu90 4:a5b51a651db7 518 case 'b':
Michu90 5:c3caf8b83e6b 519 PWM3zad+=50;
Michu90 8:dc48ce79ad59 520 //PWM2zad+=50;
Michu90 8:dc48ce79ad59 521 //PWM3zad+=50;
Michu90 8:dc48ce79ad59 522 //PWM1zad+=50;
Michu90 8:dc48ce79ad59 523 //PWM1zad=1.026*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 524 PWM1zad=1.035*(PWM3zad-10000)+10000;
Michu90 8:dc48ce79ad59 525
Michu90 8:dc48ce79ad59 526 //PWM1zad+=50;
Michu90 5:c3caf8b83e6b 527 if(PWM1zad>=20000){
Michu90 5:c3caf8b83e6b 528 PWM1zad=20000;
Michu90 5:c3caf8b83e6b 529 PWM2zad=20000;
Michu90 5:c3caf8b83e6b 530 PWM3zad=20000;
Michu90 5:c3caf8b83e6b 531 PWM4zad=20000;
Michu90 5:c3caf8b83e6b 532 }
Michu90 5:c3caf8b83e6b 533 //ustawianie
Michu90 5:c3caf8b83e6b 534 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 535 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 536 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 537 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 538 znak2=0;
Michu90 5:c3caf8b83e6b 539 break;
Michu90 5:c3caf8b83e6b 540
Michu90 5:c3caf8b83e6b 541 case 'x':
Michu90 8:dc48ce79ad59 542 sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
Michu90 6:8cc6df266363 543 pc.printf(buff);
Michu90 6:8cc6df266363 544 wait(1.0f);
Michu90 6:8cc6df266363 545 Kp1=0;
Michu90 8:dc48ce79ad59 546 Kd1=0;
Michu90 6:8cc6df266363 547 Kp2=0;
Michu90 8:dc48ce79ad59 548 Kd2=0;
Michu90 8:dc48ce79ad59 549 Ki2=0;
Michu90 8:dc48ce79ad59 550 Ki2=0;
Michu90 5:c3caf8b83e6b 551 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 552 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 553 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 554 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 555 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 556 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 557 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 558 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 559 sprintf(buff,"Odlacz silniki\n\r");
Michu90 5:c3caf8b83e6b 560 pc.printf(buff);
Michu90 5:c3caf8b83e6b 561 wait(1.0f);
Michu90 5:c3caf8b83e6b 562 sprintf(buff,"5 \n\r");
Michu90 5:c3caf8b83e6b 563 pc.printf(buff);
Michu90 5:c3caf8b83e6b 564 wait(1.0f);
Michu90 5:c3caf8b83e6b 565 sprintf(buff,"4 \n\r");
Michu90 5:c3caf8b83e6b 566 pc.printf(buff);
Michu90 5:c3caf8b83e6b 567 wait(1.0f);
Michu90 5:c3caf8b83e6b 568 sprintf(buff,"3 \n\r");
Michu90 5:c3caf8b83e6b 569 pc.printf(buff);
Michu90 5:c3caf8b83e6b 570 wait(1.0f);
Michu90 5:c3caf8b83e6b 571 sprintf(buff,"2 \n\r");
Michu90 5:c3caf8b83e6b 572 pc.printf(buff);
Michu90 5:c3caf8b83e6b 573 wait(1.0f);
Michu90 5:c3caf8b83e6b 574 sprintf(buff,"1 \n\r");
Michu90 5:c3caf8b83e6b 575 pc.printf(buff);
Michu90 5:c3caf8b83e6b 576 wait(1.0f);
Michu90 5:c3caf8b83e6b 577 sprintf(buff,"GO! \n\r");
Michu90 4:a5b51a651db7 578 pc.printf(buff);
Michu90 4:a5b51a651db7 579 break;
Michu90 4:a5b51a651db7 580
Michu90 6:8cc6df266363 581 case 'c':
Michu90 8:dc48ce79ad59 582 Kd1-=0.5;
Michu90 8:dc48ce79ad59 583 Kd2-=0.5;
Michu90 6:8cc6df266363 584 break;
Michu90 6:8cc6df266363 585 case 'd':
Michu90 8:dc48ce79ad59 586 Kd1+=0.5;
Michu90 8:dc48ce79ad59 587 Kd2+=0.5;
Michu90 6:8cc6df266363 588 break;
Michu90 6:8cc6df266363 589 case 'e':
Michu90 6:8cc6df266363 590 Kp1-=0.5;
Michu90 6:8cc6df266363 591 Kp2-=0.5;
Michu90 6:8cc6df266363 592 break;
Michu90 6:8cc6df266363 593 case 'f':
Michu90 6:8cc6df266363 594 Kp1+=0.5;
Michu90 6:8cc6df266363 595 Kp2+=0.5;
Michu90 8:dc48ce79ad59 596 break;
Michu90 8:dc48ce79ad59 597 case 'g':
Michu90 8:dc48ce79ad59 598 Ki1-=0.1;
Michu90 8:dc48ce79ad59 599 Ki2-=0.1;
Michu90 8:dc48ce79ad59 600 break;
Michu90 8:dc48ce79ad59 601 case 'h':
Michu90 8:dc48ce79ad59 602 Ki1+=0.1;
Michu90 8:dc48ce79ad59 603 Ki2+=0.1;
Michu90 8:dc48ce79ad59 604 break;
Michu90 5:c3caf8b83e6b 605 }
Michu90 4:a5b51a651db7 606 }
Michu90 6:8cc6df266363 607 //myled2 = !myled2;
Igor_W 2:9e6ed6302c78 608 }
mbed_official 0:50d2b9c62765 609 }