Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@7:2ba30a0cdc16, 2015-01-15 (annotated)

Committer:
Michu90
Date:
Thu Jan 15 07:46:43 2015 +0000
Revision:
7:2ba30a0cdc16
Parent:
6:8cc6df266363
Child:
8:dc48ce79ad59
poleci?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:50d2b9c62765 1 #include "mbed.h"
Igor_W 3:1425359662e4 2 #include "FastPWM.h"
Michu90 5:c3caf8b83e6b 3 #include "kalman.h"
Michu90 5:c3caf8b83e6b 4 #include "Offsets.h"
Michu90 5:c3caf8b83e6b 5 #include "stdio.h"
Michu90 5:c3caf8b83e6b 6 #include "IMU.h"
Michu90 5:c3caf8b83e6b 7
Igor_W 2:9e6ed6302c78 8 #define PWM_period 2500
Michu90 5:c3caf8b83e6b 9 #define M_PI 3.141592
Michu90 5:c3caf8b83e6b 10 #define M_PI2 1.570796
Michu90 5:c3caf8b83e6b 11 #define dt 0.005
Michu90 5:c3caf8b83e6b 12
Michu90 5:c3caf8b83e6b 13
Michu90 5:c3caf8b83e6b 14 //kalman
Michu90 5:c3caf8b83e6b 15 // Structs for containing filter data
Michu90 5:c3caf8b83e6b 16 kalman_data pitch_data;
Michu90 5:c3caf8b83e6b 17 kalman_data roll_data;
Michu90 5:c3caf8b83e6b 18
Michu90 5:c3caf8b83e6b 19 kalman_data pitch_data2;
Michu90 5:c3caf8b83e6b 20 kalman_data roll_data2;
Michu90 5:c3caf8b83e6b 21
Michu90 5:c3caf8b83e6b 22 DigitalOut myled(PTA2);
Michu90 5:c3caf8b83e6b 23 DigitalOut myled2(PTA1);
Michu90 5:c3caf8b83e6b 24 Serial pc(USBTX, USBRX);
Michu90 5:c3caf8b83e6b 25 Serial bluetooth(D1, D0);
Michu90 5:c3caf8b83e6b 26 IMU imu(PTE25,PTE24);
Michu90 5:c3caf8b83e6b 27 Offsets off;
Michu90 5:c3caf8b83e6b 28
Michu90 5:c3caf8b83e6b 29 FastPWM M1(D10);
Michu90 5:c3caf8b83e6b 30 FastPWM M2(D11);
Michu90 5:c3caf8b83e6b 31 FastPWM M3(D12);
Michu90 5:c3caf8b83e6b 32 FastPWM M4(D13);
Igor_W 2:9e6ed6302c78 33
Igor_W 2:9e6ed6302c78 34
Michu90 5:c3caf8b83e6b 35 Ticker triger1; //przerwanie filtracji
Michu90 5:c3caf8b83e6b 36 //Ticker triger2; //przerwanie wysyłania danych
Igor_W 2:9e6ed6302c78 37
Michu90 5:c3caf8b83e6b 38 float d[9];
Michu90 5:c3caf8b83e6b 39 double D[9];
Michu90 5:c3caf8b83e6b 40 float o[3];
Michu90 5:c3caf8b83e6b 41 float O[3];
Michu90 5:c3caf8b83e6b 42 char buff[160];
Michu90 5:c3caf8b83e6b 43 float r,katx,katy;
Michu90 5:c3caf8b83e6b 44 float rbut,katxbut,katybut;
Michu90 5:c3caf8b83e6b 45 float pitch, roll;
Michu90 5:c3caf8b83e6b 46 float pitch2, roll2;
Michu90 7:2ba30a0cdc16 47 float pitchE, rollE;
Michu90 5:c3caf8b83e6b 48 double i;
Michu90 5:c3caf8b83e6b 49 float offsetGyro[3];
Michu90 5:c3caf8b83e6b 50 char odczyt[20];
Michu90 4:a5b51a651db7 51 char znak;
Michu90 4:a5b51a651db7 52 char znak2;
Michu90 7:2ba30a0cdc16 53 float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
Michu90 6:8cc6df266363 54 float U1,U2,U3;
Michu90 6:8cc6df266363 55 float Om1,Om2,Om3,Om4;
Michu90 6:8cc6df266363 56 float wyp1,wyp2,wyp3,wyp4;
Michu90 5:c3caf8b83e6b 57
Michu90 5:c3caf8b83e6b 58 double PWM1zad;
Michu90 5:c3caf8b83e6b 59 double PWM2zad;
Michu90 5:c3caf8b83e6b 60 double PWM3zad;
Michu90 5:c3caf8b83e6b 61 double PWM4zad;
Michu90 5:c3caf8b83e6b 62 double valPWM1;
Michu90 5:c3caf8b83e6b 63 double valPWM2;
Michu90 5:c3caf8b83e6b 64 double valPWM3;
Michu90 5:c3caf8b83e6b 65 double valPWM4;
Michu90 5:c3caf8b83e6b 66
Michu90 5:c3caf8b83e6b 67 void task1()
Michu90 5:c3caf8b83e6b 68 {
Michu90 7:2ba30a0cdc16 69 //myled = !myled;
Michu90 5:c3caf8b83e6b 70 imu.readData(d);
Michu90 5:c3caf8b83e6b 71 imu.filterData(d, D);
Michu90 5:c3caf8b83e6b 72 off.offsetData(d,offsetGyro,o);
Michu90 5:c3caf8b83e6b 73 off.offsetData2(D,offsetGyro,O);
Michu90 5:c3caf8b83e6b 74
Michu90 5:c3caf8b83e6b 75 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 5:c3caf8b83e6b 76 katx = acos(d[4]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 77 katy = acos(d[3]/r)-M_PI2;
Michu90 5:c3caf8b83e6b 78
Michu90 5:c3caf8b83e6b 79 rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
Michu90 5:c3caf8b83e6b 80 katxbut = acos(D[4]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 81 katybut = acos(D[3]/rbut)-M_PI2;
Michu90 5:c3caf8b83e6b 82
Michu90 5:c3caf8b83e6b 83
Michu90 5:c3caf8b83e6b 84 //Filtr Kalmana
Michu90 5:c3caf8b83e6b 85 kalman_innovate(&pitch_data, katx, o[0]);
Michu90 5:c3caf8b83e6b 86 kalman_innovate(&roll_data, -katy, o[1]);
Michu90 5:c3caf8b83e6b 87 pitch = pitch_data.x1;
Michu90 5:c3caf8b83e6b 88 roll = roll_data.x1;
Michu90 5:c3caf8b83e6b 89
Michu90 5:c3caf8b83e6b 90 //Filtr Kalmana butterworth 2nd
Michu90 5:c3caf8b83e6b 91 kalman_innovate(&pitch_data2, katxbut, O[0]);
Michu90 5:c3caf8b83e6b 92 kalman_innovate(&roll_data2, -katybut, O[1]);
Michu90 5:c3caf8b83e6b 93 pitch2 = pitch_data2.x1;
Michu90 5:c3caf8b83e6b 94 roll2 = roll_data2.x1;
Michu90 5:c3caf8b83e6b 95
Michu90 7:2ba30a0cdc16 96 //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
Michu90 7:2ba30a0cdc16 97 //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
Michu90 7:2ba30a0cdc16 98 //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
Michu90 6:8cc6df266363 99
Michu90 7:2ba30a0cdc16 100 //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 101 //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 102 //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 103 //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
Michu90 6:8cc6df266363 104
Michu90 7:2ba30a0cdc16 105 /*wyp1 = sqrt(Om1)*13.17523+10000;
Michu90 6:8cc6df266363 106 wyp2 = sqrt(Om2)*13.17523+10000;
Michu90 6:8cc6df266363 107 wyp3 = sqrt(Om3)*13.17523+10000;
Michu90 7:2ba30a0cdc16 108 wyp4 = sqrt(Om4)*13.17523+10000;*/
Michu90 6:8cc6df266363 109
Michu90 6:8cc6df266363 110
Michu90 7:2ba30a0cdc16 111 // *********************** DISCRETE PID CONTROLLER **********************
Michu90 7:2ba30a0cdc16 112 // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
Michu90 7:2ba30a0cdc16 113 // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
Michu90 7:2ba30a0cdc16 114 // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
Michu90 7:2ba30a0cdc16 115 //omega1^2=(B1/b*PWM1zad+C1/b)-U2/2bl+U3/4d
Michu90 7:2ba30a0cdc16 116 //omega2^2=(B/b*PWM2zad+C/b)+U1/2bl-U3/4d
Michu90 7:2ba30a0cdc16 117 //omega3^2=(B3/b*PWM3zad+C3/b)+U2/2bl+U3/4d
Michu90 7:2ba30a0cdc16 118 //omega4^2=(B/b*PWM4zad+C/b)-U1/2bl-U3/4d
Michu90 7:2ba30a0cdc16 119 //wyp1=b/B1*omega1^2-C1/B1
Michu90 7:2ba30a0cdc16 120 //wyp2=b/B*omega2^2-C/B
Michu90 7:2ba30a0cdc16 121 //wyp3=b/B3*omega3^2-C3/B3
Michu90 7:2ba30a0cdc16 122 //wyp4=b/B*omega4^2-C/B
Michu90 7:2ba30a0cdc16 123
Michu90 7:2ba30a0cdc16 124 //b=0.000015
Michu90 7:2ba30a0cdc16 125 //B1=0.000776646
Michu90 7:2ba30a0cdc16 126 //C1=-0.334958973
Michu90 7:2ba30a0cdc16 127 //l=0.3
Michu90 7:2ba30a0cdc16 128 //d=0.000033
Michu90 7:2ba30a0cdc16 129 //2bl=0.0000092780
Michu90 7:2ba30a0cdc16 130 //4d=0.000132
Michu90 7:2ba30a0cdc16 131 //B1/b=50.22498189
Michu90 7:2ba30a0cdc16 132 //C1/b=-21611.4954
Michu90 7:2ba30a0cdc16 133 //b/B1=0.01991041
Michu90 7:2ba30a0cdc16 134 //C1/B1=-431.2892625
Michu90 7:2ba30a0cdc16 135
Michu90 7:2ba30a0cdc16 136 //B3/b=49.90897978
Michu90 7:2ba30a0cdc16 137 //C3/b=-25683.36221
Michu90 7:2ba30a0cdc16 138 //b/B3=0.020036474
Michu90 7:2ba30a0cdc16 139 //C3/B3=-514.6040317
Michu90 7:2ba30a0cdc16 140
Michu90 7:2ba30a0cdc16 141
Michu90 7:2ba30a0cdc16 142
Michu90 6:8cc6df266363 143
Michu90 7:2ba30a0cdc16 144 pitchE=pitchE+(0-pitch2);
Michu90 7:2ba30a0cdc16 145 if(pitchE>3) pitchE=3;
Michu90 7:2ba30a0cdc16 146 Kd1=Kp1*Td1/T;
Michu90 7:2ba30a0cdc16 147 Ki1=Kp1*T/Ti1;
Michu90 7:2ba30a0cdc16 148 if(Ti1==0)Ki1=0;
Michu90 6:8cc6df266363 149
Michu90 7:2ba30a0cdc16 150 rollE=rollE+(0-roll2);
Michu90 7:2ba30a0cdc16 151 if(rollE>3) rollE=3;
Michu90 7:2ba30a0cdc16 152 Kd2=Kp2*Td2/T;
Michu90 7:2ba30a0cdc16 153 Ki2=Kp2*T/Ti2;
Michu90 7:2ba30a0cdc16 154 if(Ti2==0)Ki2=0;
Michu90 7:2ba30a0cdc16 155
Michu90 7:2ba30a0cdc16 156 /* yawE=yawE+(0-roll2);
Michu90 7:2ba30a0cdc16 157 if(yawE>3) yawE=3;*/
Michu90 7:2ba30a0cdc16 158 Kd3=Kp3*Td3/T;
Michu90 7:2ba30a0cdc16 159 Ki3=Kp3*T/Ti3;
Michu90 7:2ba30a0cdc16 160 if(Ti3==0)Ki3=0;
Michu90 7:2ba30a0cdc16 161
Michu90 7:2ba30a0cdc16 162 U1 = 0.0173*(Kp1*(0-pitch2)+Kd1*(0-O[0])+Ki1*pitchE);
Michu90 7:2ba30a0cdc16 163 U2 = 0.0169*(Kp2*(0-roll2)+Kp2*Td2/T*(0-O[1])+Kp2*T/Ti2*rollE);
Michu90 7:2ba30a0cdc16 164 U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
Michu90 7:2ba30a0cdc16 165
Michu90 7:2ba30a0cdc16 166 Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ; //kwadraty
Michu90 7:2ba30a0cdc16 167 Om2 = 50.22498189*PWM2zad+(-21611.4954)+U1/0.0000092780 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 168 Om3 = 49.90897978*PWM3zad+(-25683.36221)+U2/0.0000092780 + U3/0.000132 ;
Michu90 7:2ba30a0cdc16 169 Om4 = 50.22498189*PWM4zad+(-21611.4954)-U1/0.0000092780 - U3/0.000132 ;
Michu90 7:2ba30a0cdc16 170
Michu90 7:2ba30a0cdc16 171 wyp1=0.01991041*Om1+(-431.2892625);
Michu90 7:2ba30a0cdc16 172 wyp2=0.01991041*Om2+(-431.2892625);
Michu90 7:2ba30a0cdc16 173 wyp3=0.020036474*Om3+(-514.6040317);
Michu90 7:2ba30a0cdc16 174 wyp4=0.01991041*Om4+(-431.2892625);
Michu90 7:2ba30a0cdc16 175
Michu90 6:8cc6df266363 176 if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
Michu90 6:8cc6df266363 177 if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
Michu90 6:8cc6df266363 178 if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
Michu90 6:8cc6df266363 179
Michu90 6:8cc6df266363 180 if(wyp2<=10001 & wyp2>40001) valPWM2=10000;
Michu90 6:8cc6df266363 181 if(wyp2>=20000 & wyp2<40000) valPWM2=20000;
Michu90 6:8cc6df266363 182 if(wyp2>10000 & wyp2<20000) valPWM2=(int)wyp2;
Michu90 6:8cc6df266363 183
Michu90 6:8cc6df266363 184 if(wyp3<=10001 | wyp3>40001) valPWM3=10000;
Michu90 6:8cc6df266363 185 if(wyp3>=20000 & wyp3<40000) valPWM3=20000;
Michu90 6:8cc6df266363 186 if(wyp3>10100 & wyp3<20000) valPWM3=(int)wyp3;
Michu90 6:8cc6df266363 187
Michu90 6:8cc6df266363 188 if(wyp4<=10001 & wyp4>40001) valPWM4=10000;
Michu90 6:8cc6df266363 189 if(wyp4>=20000 & wyp4<40000) valPWM4=20000;
Michu90 6:8cc6df266363 190 if(wyp4>10000 & wyp4<20000) valPWM4=(int)wyp4;
Michu90 6:8cc6df266363 191
Michu90 5:c3caf8b83e6b 192
Michu90 5:c3caf8b83e6b 193 //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 194 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 195 //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 196 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 197 //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 198 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 199 //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 200 //pc.printf(buff);
Michu90 5:c3caf8b83e6b 201
Michu90 6:8cc6df266363 202 M1.pulsewidth_us(valPWM1);
Michu90 6:8cc6df266363 203 M2.pulsewidth_us(valPWM2);
Michu90 6:8cc6df266363 204 M3.pulsewidth_us(valPWM3);
Michu90 6:8cc6df266363 205 M4.pulsewidth_us(valPWM4);
Michu90 6:8cc6df266363 206
Michu90 6:8cc6df266363 207
Michu90 7:2ba30a0cdc16 208 //myled = !myled;
Michu90 5:c3caf8b83e6b 209 }
Michu90 5:c3caf8b83e6b 210
Michu90 5:c3caf8b83e6b 211
Michu90 5:c3caf8b83e6b 212 void task2()
Michu90 5:c3caf8b83e6b 213 {
Michu90 5:c3caf8b83e6b 214 /*
Michu90 5:c3caf8b83e6b 215 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 216 pc.printf(buff);
Michu90 5:c3caf8b83e6b 217 myled2 = !myled2;*/
Michu90 5:c3caf8b83e6b 218 }
Michu90 5:c3caf8b83e6b 219
Michu90 5:c3caf8b83e6b 220
Michu90 5:c3caf8b83e6b 221
Michu90 4:a5b51a651db7 222
mbed_official 0:50d2b9c62765 223 int main() {
Igor_W 2:9e6ed6302c78 224
Michu90 4:a5b51a651db7 225 pc.baud(115200);
Michu90 4:a5b51a651db7 226 bluetooth.baud(19200);
Michu90 5:c3caf8b83e6b 227 imu.init();
Michu90 5:c3caf8b83e6b 228 kalman_init(&pitch_data);
Michu90 5:c3caf8b83e6b 229 kalman_init(&roll_data);
Michu90 5:c3caf8b83e6b 230 kalman_init(&pitch_data2);
Michu90 5:c3caf8b83e6b 231 kalman_init(&roll_data2);
Igor_W 3:1425359662e4 232
Michu90 4:a5b51a651db7 233 sprintf(buff, "Hello: \n\r");
Michu90 4:a5b51a651db7 234 pc.printf(buff);
Michu90 4:a5b51a651db7 235
Michu90 6:8cc6df266363 236 off.setOffsets(offsetGyro, pc, imu);
Michu90 5:c3caf8b83e6b 237
Michu90 5:c3caf8b83e6b 238
Michu90 5:c3caf8b83e6b 239
Michu90 5:c3caf8b83e6b 240 triger1.attach(&task1, 0.005);
Michu90 5:c3caf8b83e6b 241 //triger2.attach(&task2, 0.005);
Michu90 5:c3caf8b83e6b 242 i=1000;
Michu90 5:c3caf8b83e6b 243
Igor_W 3:1425359662e4 244
Michu90 5:c3caf8b83e6b 245 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 246 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 247 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 248 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 249
Michu90 5:c3caf8b83e6b 250 M1.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 251 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 252 M2.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 253 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 254 M3.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 255 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 256 M4.period_us(PWM_period);
Michu90 5:c3caf8b83e6b 257 M4.pulsewidth_us(PWM4zad);
Michu90 4:a5b51a651db7 258
Michu90 6:8cc6df266363 259 Kp1=0;
Michu90 6:8cc6df266363 260 Kp2=0;
Michu90 6:8cc6df266363 261 Kp3=0;
Michu90 7:2ba30a0cdc16 262 Td1=0;
Michu90 7:2ba30a0cdc16 263 Td2=0;
Michu90 7:2ba30a0cdc16 264 Td3=0;
Michu90 7:2ba30a0cdc16 265 Ti1=0;
Michu90 7:2ba30a0cdc16 266 Ti2=0;
Michu90 7:2ba30a0cdc16 267 Ti3=0;
Michu90 7:2ba30a0cdc16 268 T=0.005;
Michu90 6:8cc6df266363 269
Michu90 6:8cc6df266363 270
Igor_W 2:9e6ed6302c78 271 while(1) {
Michu90 6:8cc6df266363 272
Michu90 7:2ba30a0cdc16 273 //myled2 = !myled2;
Michu90 5:c3caf8b83e6b 274 //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 275 //pc.printf(buff);
Michu90 7:2ba30a0cdc16 276 //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1, valPWM2, valPWM3, valPWM4);
Michu90 7:2ba30a0cdc16 277 //pc.printf(buff);
Michu90 7:2ba30a0cdc16 278 sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, -katy*180/M_PI,(O[1]*180/M_PI),wyp1,wyp2,wyp3,wyp4);
Michu90 6:8cc6df266363 279 pc.printf(buff);
Michu90 6:8cc6df266363 280 //sprintf(buff, "%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI);
Michu90 6:8cc6df266363 281 //pc.printf(buff);
Michu90 6:8cc6df266363 282 //sprintf(buff, "%f,%f,%f,%f\n\r", valPWM1, valPWM2, valPWM3, valPWM4);
Michu90 6:8cc6df266363 283 //pc.printf(buff);
Michu90 6:8cc6df266363 284
Michu90 7:2ba30a0cdc16 285 //myled2 = !myled2;
Michu90 6:8cc6df266363 286
Michu90 5:c3caf8b83e6b 287
Michu90 4:a5b51a651db7 288
Michu90 4:a5b51a651db7 289 if(pc.readable()){
Michu90 4:a5b51a651db7 290 znak=pc.getc();
Michu90 4:a5b51a651db7 291 switch (znak){
Michu90 5:c3caf8b83e6b 292
Michu90 4:a5b51a651db7 293 case 'p':
Michu90 4:a5b51a651db7 294 sprintf(buff, "odczytany znak: %c\n\r",znak);
Michu90 4:a5b51a651db7 295 pc.printf(buff);
Michu90 4:a5b51a651db7 296 break;
Michu90 7:2ba30a0cdc16 297 /*case 'u':
Michu90 7:2ba30a0cdc16 298 PWM1zad+=10;
Michu90 7:2ba30a0cdc16 299 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 300 break;
Michu90 7:2ba30a0cdc16 301 case 'j':
Michu90 7:2ba30a0cdc16 302 PWM1zad-=10;
Michu90 7:2ba30a0cdc16 303 M1.pulsewidth_us(PWM1zad);
Michu90 7:2ba30a0cdc16 304 break;*/
Michu90 4:a5b51a651db7 305
Michu90 4:a5b51a651db7 306 }
Michu90 4:a5b51a651db7 307
Michu90 5:c3caf8b83e6b 308 znak=0;
Michu90 4:a5b51a651db7 309 }
Michu90 5:c3caf8b83e6b 310
Michu90 4:a5b51a651db7 311
Michu90 4:a5b51a651db7 312 if(bluetooth.readable()){
Michu90 4:a5b51a651db7 313
Michu90 4:a5b51a651db7 314 znak2=bluetooth.getc();
Michu90 4:a5b51a651db7 315
Michu90 5:c3caf8b83e6b 316 switch (znak2){
Michu90 4:a5b51a651db7 317 case 'a':
Michu90 5:c3caf8b83e6b 318 PWM1zad-=50;
Michu90 5:c3caf8b83e6b 319 PWM2zad-=50;
Michu90 5:c3caf8b83e6b 320 PWM3zad-=50;
Michu90 5:c3caf8b83e6b 321 PWM4zad-=50;
Michu90 5:c3caf8b83e6b 322 if(PWM1zad<10000){
Michu90 5:c3caf8b83e6b 323 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 324 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 325 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 326 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 327 }
Michu90 5:c3caf8b83e6b 328 //ustawianie
Michu90 5:c3caf8b83e6b 329 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 330 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 331 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 332 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 333 znak2=0;
Michu90 4:a5b51a651db7 334 break;
Michu90 4:a5b51a651db7 335
Michu90 4:a5b51a651db7 336 case 'b':
Michu90 5:c3caf8b83e6b 337 PWM1zad+=50;
Michu90 5:c3caf8b83e6b 338 PWM2zad+=50;
Michu90 5:c3caf8b83e6b 339 PWM3zad+=50;
Michu90 5:c3caf8b83e6b 340 PWM4zad+=50;
Michu90 5:c3caf8b83e6b 341 if(PWM1zad>=20000){
Michu90 5:c3caf8b83e6b 342 PWM1zad=20000;
Michu90 5:c3caf8b83e6b 343 PWM2zad=20000;
Michu90 5:c3caf8b83e6b 344 PWM3zad=20000;
Michu90 5:c3caf8b83e6b 345 PWM4zad=20000;
Michu90 5:c3caf8b83e6b 346 }
Michu90 5:c3caf8b83e6b 347 //ustawianie
Michu90 5:c3caf8b83e6b 348 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 349 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 350 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 351 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 352 znak2=0;
Michu90 5:c3caf8b83e6b 353 break;
Michu90 5:c3caf8b83e6b 354
Michu90 5:c3caf8b83e6b 355 case 'x':
Michu90 7:2ba30a0cdc16 356 sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Td2);
Michu90 6:8cc6df266363 357 pc.printf(buff);
Michu90 6:8cc6df266363 358 wait(1.0f);
Michu90 6:8cc6df266363 359 Kp1=0;
Michu90 7:2ba30a0cdc16 360 Td1=0;
Michu90 6:8cc6df266363 361 Kp2=0;
Michu90 7:2ba30a0cdc16 362 Td2=0;
Michu90 5:c3caf8b83e6b 363 PWM1zad=10000;
Michu90 5:c3caf8b83e6b 364 PWM2zad=10000;
Michu90 5:c3caf8b83e6b 365 PWM3zad=10000;
Michu90 5:c3caf8b83e6b 366 PWM4zad=10000;
Michu90 5:c3caf8b83e6b 367 M1.pulsewidth_us(PWM1zad);
Michu90 5:c3caf8b83e6b 368 M2.pulsewidth_us(PWM2zad);
Michu90 5:c3caf8b83e6b 369 M3.pulsewidth_us(PWM3zad);
Michu90 5:c3caf8b83e6b 370 M4.pulsewidth_us(PWM4zad);
Michu90 5:c3caf8b83e6b 371 sprintf(buff,"Odlacz silniki\n\r");
Michu90 5:c3caf8b83e6b 372 pc.printf(buff);
Michu90 5:c3caf8b83e6b 373 wait(1.0f);
Michu90 5:c3caf8b83e6b 374 sprintf(buff,"5 \n\r");
Michu90 5:c3caf8b83e6b 375 pc.printf(buff);
Michu90 5:c3caf8b83e6b 376 wait(1.0f);
Michu90 5:c3caf8b83e6b 377 sprintf(buff,"4 \n\r");
Michu90 5:c3caf8b83e6b 378 pc.printf(buff);
Michu90 5:c3caf8b83e6b 379 wait(1.0f);
Michu90 5:c3caf8b83e6b 380 sprintf(buff,"3 \n\r");
Michu90 5:c3caf8b83e6b 381 pc.printf(buff);
Michu90 5:c3caf8b83e6b 382 wait(1.0f);
Michu90 5:c3caf8b83e6b 383 sprintf(buff,"2 \n\r");
Michu90 5:c3caf8b83e6b 384 pc.printf(buff);
Michu90 5:c3caf8b83e6b 385 wait(1.0f);
Michu90 5:c3caf8b83e6b 386 sprintf(buff,"1 \n\r");
Michu90 5:c3caf8b83e6b 387 pc.printf(buff);
Michu90 5:c3caf8b83e6b 388 wait(1.0f);
Michu90 5:c3caf8b83e6b 389 sprintf(buff,"GO! \n\r");
Michu90 4:a5b51a651db7 390 pc.printf(buff);
Michu90 4:a5b51a651db7 391 break;
Michu90 4:a5b51a651db7 392
Michu90 6:8cc6df266363 393 case 'c':
Michu90 7:2ba30a0cdc16 394 Td1-=0.001;
Michu90 7:2ba30a0cdc16 395 Td2-=0.001;
Michu90 6:8cc6df266363 396 break;
Michu90 6:8cc6df266363 397 case 'd':
Michu90 7:2ba30a0cdc16 398 Td1+=0.001;
Michu90 7:2ba30a0cdc16 399 Td2+=0.001;
Michu90 6:8cc6df266363 400 break;
Michu90 6:8cc6df266363 401 case 'e':
Michu90 6:8cc6df266363 402 Kp1-=0.5;
Michu90 6:8cc6df266363 403 Kp2-=0.5;
Michu90 6:8cc6df266363 404 break;
Michu90 6:8cc6df266363 405 case 'f':
Michu90 6:8cc6df266363 406 Kp1+=0.5;
Michu90 6:8cc6df266363 407 Kp2+=0.5;
Michu90 6:8cc6df266363 408 break;
Michu90 5:c3caf8b83e6b 409 }
Michu90 4:a5b51a651db7 410 }
Michu90 6:8cc6df266363 411 //myled2 = !myled2;
Igor_W 2:9e6ed6302c78 412 }
mbed_official 0:50d2b9c62765 413 }