Mechatronics / 2015_04_17_dydakt

Dydaktyka

Dependencies:   FastPWM mbed-src

Fork of 2015_04_17_quadro_bez_sterowania by Quadrocopter

main.cpp@6:8cc6df266363, 2015-01-07 (annotated)

Committer:
Michu90
Date:
Wed Jan 07 13:44:34 2015 +0000
Revision:
6:8cc6df266363
Parent:
5:c3caf8b83e6b
Child:
7:2ba30a0cdc16
Happy New Year!

Who changed what in which revision?

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