Mechatronics
Dydaktyka
Dependencies: FastPWM mbed-src
Fork of
2015_04_17_quadro_bez_sterowania
by 
Quadrocopter
Diff: main.cpp
- Revision:
- 12:e955cc086c42
- Parent:
- 11:38db3ed41f13
--- a/main.cpp Fri Apr 17 07:48:05 2015 +0000
+++ b/main.cpp Fri Apr 17 14:25:25 2015 +0000
@@ -17,6 +17,10 @@
#define filtr3Sygnal d[5]
+
+#define zad 1
+
+
//kalman
// Structs for containing filter data
kalman_data pitch_data;
@@ -29,8 +33,11 @@
DigitalOut myled2(PTA1);
Serial pc(USBTX, USBRX);
Serial bluetooth(D1, D0);
+
IMU imu(PTE25,PTE24);
+
Offsets off;
+
InterruptIn event(PTB23);
FastPWM M1(D10);
@@ -39,8 +46,8 @@
FastPWM M4(D13);
Ticker triger1; //przerwanie filtracji
-Ticker triger2; //kalibracja accelero
-Ticker triger3; //kalibracja zyro
+//Ticker triger2; //kalibracja accelero
+//Ticker triger3; //kalibracja zyro
Ticker tachometer;
float d[9];
@@ -48,39 +55,33 @@
float o[3];
float O[3];
char buff[160];
-float r,katx,katy,katz;
+float r,katx,katy,katz,katzyro;
float rbut,katxbut,katybut;
float pitch, roll, yaw;
float pitch2, roll2;
-float pitchE, rollE, yawE;
+float pitchma, rollma;
+
double i;
float offsetGyro[3];
float offsetMagneto;
float yawOff;
char odczyt[20];
+
+
char znak;
char znak2;
-float Kp1,Td1,Kd1,Kp2,Td2,Kd2,Kp3,Td3,Kd3,Ti1,Ki1,Ti2,Ki2,Ti3,Ki3,T;
-float U1,U2,U3;
-float Om1,Om2,Om3,Om4;
-float wyp1,wyp2,wyp3,wyp4;
+
int impulsA;
-
double PWM1zad;
double PWM2zad;
double PWM3zad;
double PWM4zad;
-double valPWM1;
-double valPWM2;
-double valPWM3;
-double valPWM4;
-float T1zad,T2zad,T3zad,T4zad;
+
//zmienne tymczasowe
float katxzyro,katyzyro,katzzyro;
float fkompl;
-float xmin, ymin, zmin;
-float xmax, ymax, zmax;
+
float filtrBufor[filtrWymiar];
int filtrZmienna=0;
float filtrSuma=0;
@@ -101,6 +102,8 @@
float kalmanpitchdryf;
int RPMtach;
+int zadanie;
+/*
float PWM4zadBuf[5];
float PWM4zadPoprz;
float PWM4zadWyn;
@@ -109,31 +112,24 @@
int RPMtachAktualny;
float RPMtachWyn;
int RPMtachWynPoprz;
+*/
+
int iTach;
bool TachFlaga;
bool FlagaPom;
-int Omegazad[4];
-int Omegamax[4][9];
-float a1buf[4][9];
-float a2buf[4][9];
-int ister;
-int jster;
bool flagaster;
-float a1[4];
-float a2[4];
void rise(void)
{
- //myled = !myled;
if(TachFlaga==1)impulsA++;
}
-
void task1()
{
- //myled = !myled;
imu.readData(d);
+
+
imu.filterData(d, D);
off.offsetData(d,offsetGyro,o);
off.offsetData2(D,offsetGyro,O);
@@ -144,7 +140,11 @@
yaw = atan2(d[7],d[6])+4.98333*M_PI/180;
katz = katz+(o[2]*dt);
+
+ katzyro=katzyro+o[4]*dt;
+
off.offsetMagneto(&yaw,&offsetMagneto,&yawOff);
+
// Correct for heading < 0deg and heading > 360deg
/*if(yawOff < 0){
yawOff+= 2 * M_PI;
@@ -153,14 +153,7 @@
if(yawOff > 2 * M_PI){
yawOff-= 2 * M_PI;
}*/
-
- /*rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
- katxbut = acos(D[4]/rbut)-M_PI2;
- katybut = acos(D[3]/rbut)-M_PI2;*/
-
- //katxzyro = katxzyro + o[0]*dt;
- //fkompl = katyzyro*0.95+ (-katy)*0.05;
-
+
//Filtr Kalmana
kalman_innovate(&pitch_data, katx, o[0]);
kalman_innovate(&roll_data, -katy, o[1]);
@@ -168,17 +161,10 @@
kalmanpitchzyro = pitch_data.x2;
roll = roll_data.x1;
kalmanrollzyro = roll_data.x2;
-
- /*
- //Filtr Kalmana butterworth 2nd
- kalman_innovate(&pitch_data2, katxbut, O[0]);
- kalman_innovate(&roll_data2, -katybut, O[1]);
- pitch2 = pitch_data2.x1;
- kalmanpitchzyrobut = pitch_data2.x2;
- roll2 = roll_data2.x1;
- kalmanrollzyrobut = roll_data2.x2;*/
+
- if (filtrZmienna < 0) {
+ //moving avarage
+ if (filtrZmienna < 0) {
filtrWynik = filtrSygnal;
filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
@@ -199,220 +185,7 @@
if (filtrZmienna == filtrWymiar) {
filtrZmienna = 0;
}
-
- roll = filtrWynik;
-
- if (filtr2Zmienna < 0) {
- filtr2Wynik = filtr2Sygnal;
- filtr2Bufor[filtr2Zmienna + filtr2Wymiar] = filtr2Sygnal;
- filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna + filtr2Wymiar];
- } else {
- if (filtr2Zmienna == filtr2Wymiar - 1) {
- filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
- filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
- filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
- filtr2Wynik = filtr2Suma / filtr2Wymiar;
- } else {
- filtr2Suma = filtr2Suma - filtr2Bufor[filtr2Zmienna];
- filtr2Bufor[filtr2Zmienna] = filtr2Sygnal;
- filtr2Suma = filtr2Suma + filtr2Bufor[filtr2Zmienna];
- filtr2Wynik = filtr2Suma / filtr2Wymiar;
- }
- }
- filtr2Zmienna++;
- if (filtr2Zmienna == filtr2Wymiar) {
- filtr2Zmienna = 0;
- }
- pitch=filtr2Wynik;
-
- //U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
- //U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
- //U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
-
- //Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ; //kwadraty
- //Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
- //Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
- //Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
-
- /*wyp1 = sqrt(Om1)*13.17523+10000;
- wyp2 = sqrt(Om2)*13.17523+10000;
- wyp3 = sqrt(Om3)*13.17523+10000;
- wyp4 = sqrt(Om4)*13.17523+10000;*/
-
-
- // *********************** DISCRETE PID CONTROLLER **********************
- //stale
- //b=0.000015
- //B1=0.000776646
- //C1=-0.334958973
- //l=0.3
- //d=0.000033
- //4d=0.000132
-
- //B1/b1=50.22498189
- //C1/b1=-21611.4954
- //b1/B1=0.01991041
- //C1/B1=-431.2892625
- //1/B1=1287.588322
-
- //2b2l=0.000009424
- //B2/b2=51.130533
- //C2/b2=-30717.00651
- //b2/B2=0.019557786
- //C2/B2=-600.7566265
-
- //2b3l=0.000009139
- //B3/b3=50.66613192
- //C3/b3=-26072.99576
- //b3/B3=0.01973705
- //C3/B3=-514.6040317
-
- //2b4l=0.000009276
- //B4/b4=50.95680893
- //C4/b4=-28979.76588
- //b4/B4=0.019624463
- //C4/B4=-568.7123367
-
- //stare 2b*l
- //0.0000092780
- //0.000009424
- //0.000009139
- //0.000009276
-
- //2b1l=0.000000096789585
- //2b2l=0.000000095889323
- //2b3l=0.000000097529031
- //2b4l=0.000000093579744
-
- // U1=Ixx*(Kp1*(katzad-kat)+Kd1*(omegazad-omega)+Ki1*sumauchyb)
- // U2=Iyy*(Kp2*(katzad-kat)+Kd2*(omegazad-omega)+Ki2*sumauchyb)
- // U3=Izz*(Kp3*(katzad-kat)+Kd3*(omegazad-omega)+Ki3*sumauchyb)
- //omega1^2=(B1/b1*PWM1zad+C1/b1)-U2/2b1l+U3/4d
- //omega2^2=(B2/b2*PWM2zad+C2/b2)+U1/2b2l-U3/4d
- //omega3^2=(B3/b3*PWM3zad+C3/b3)+U2/2b3l+U3/4d
- //omega4^2=(B4/b4*PWM4zad+C4/b4)-U1/2b4l-U3/4d
- //wyp1=b/B1*omega1^2-C1/B1
- //wyp2=b/B*omega2^2-C/B
- //wyp3=b/B3*omega3^2-C3/B3
- //wyp4=b/B*omega4^2-C/B
-
- //B*PWMzad+C=T
- //PWMzad=1/B*T-C/B
- /*
- PWMzad1=1287.588322*T1zad-(-431.2892625);
- PWMzad2=1245.207965*T2zad-(-600.7566265);
- PWMzad3=1295.740776*T3zad-(-514.6040317);
- PWMzad4=1269.412072*T4zad-(-568.7123367);*/
- /*
- wyp1=0.01991041*Om1-(-431.2892625)+PWM1zad;
- wyp2=0.019557786*Om2-(-600.7566265)+PWM2zad;
- wyp3=0.01973705*Om3-(-514.6040317)+PWM3zad;
- wyp4=0.019624463*Om4-(-568.7123367)+PWM4zad;
- */
-
- pitchE=pitchE+(0-pitch);
- if(pitchE>3) pitchE=3;
- if(pitchE<-3) pitchE=-3;
- //Kd1=Kp1*Td1/T;
- //if(Ti1==0){Ki1=0;}else Ki1=Kp1*T/Ti1;
-
- rollE=rollE+(0-roll2);
- if(rollE>5) rollE=5;
- if(rollE<-5) rollE=-5;
- //Kd2=Kp2*Td2/T;
- //if(Ti2==0){Ki2=0;}else Ki2=Kp2*T/Ti2;
-
- /* yawE=yawE+(0-roll2);
- if(yawE>3) yawE=3;*/
- Kd3=Kp3*Td3/T;
- //if(Ti3==0){Ki3=0;}else Ki3=Kp3*T/Ti3;
-
- //U1 = 0.0346*(Kp1*(0-pitch)+Kd1*(0-(-kalmanpitchzyro))+Ki1*pitchE);
- //U2 = 0.0338*(Kp2*(0-roll)+Kd2*(0-o[1])+Ki2*rollE);
- //U3 = 0.0666*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
-
- U1 = 0.0173*(Kp1*(0-pitch)+Kd1*(0+kalmanpitchzyro)+Ki1*pitchE);
- U2 = 0.0169*(Kp2*(0-roll)+Kd2*(0-kalmanrollzyro)+Ki2*rollE);
- U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-o[2]));//+Ki3*rollE);
-
- Om1 = Omegazad[0]*Omegazad[0]-U2/0.000000096789585 + U3/0.000132 ; //kwadraty
- Om2 = Omegazad[1]*Omegazad[1]+U1/0.000000095889323 - U3/0.000132 ;
- Om3 = Omegazad[2]*Omegazad[2]+U2/0.000000097529031 + U3/0.000132 ;
- Om4 = Omegazad[3]*Omegazad[3]-U1/0.000000093579744 - U3/0.000132 ;
-
- wyp1=(sqrt(Om1)-a2[0])/a1[0];
- wyp2=(sqrt(Om2)-a2[1])/a1[1];
- wyp3=(sqrt(Om3)-a2[2])/a1[2];
- wyp4=(sqrt(Om4)-a2[3])/a1[3];
-
- if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
- if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
- if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
-
- if(wyp2<=10001 || wyp2>40001) valPWM2=10000;
- if(wyp2>=20000 && wyp2<40000) valPWM2=20000;
- if(wyp2>10001 && wyp2<20000) valPWM2=(int)wyp2;
-
- if(wyp3<=10001 || wyp3>40001) valPWM3=10000;
- if(wyp3>=20000 && wyp3<40000) valPWM3=20000;
- if(wyp3>10001 && wyp3<20000) valPWM3=(int)wyp3;
-
- if(wyp4<=10001 || wyp4>40001) valPWM4=10000;
- if(wyp4>=20000 && wyp4<40000) valPWM4=20000;
- if(wyp4>10001 && wyp4<20000) valPWM4=(int)wyp4;
-
-
- //sprintf(buff, "%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI);
- //pc.printf(buff);
- //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));
- //pc.printf(buff);
- //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));
- //pc.printf(buff);
- //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));
- //pc.printf(buff);
-
- M1.pulsewidth_us(valPWM1);
- M2.pulsewidth_us(valPWM2);
- M3.pulsewidth_us(valPWM3);
- M4.pulsewidth_us(valPWM4);
-
- //myled = !myled;
-}
-
-/*
-void task2()
-{
-
- 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));
- pc.printf(buff);
- myled2 = !myled2;
- imu.readData(d);
-
- if (filtrZmienna < 0) {
- filtrWynik = filtrSygnal;
- filtrBufor[filtrZmienna + filtrWymiar] = filtrSygnal;
- filtrSuma = filtrSuma + filtrBufor[filtrZmienna + filtrWymiar];
- } else {
- if (filtrZmienna == filtrWymiar - 1) {
- filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
- filtrBufor[filtrZmienna] = filtrSygnal;
- filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
- filtrWynik = filtrSuma / filtrWymiar;
- } else {
- filtrSuma = filtrSuma - filtrBufor[filtrZmienna];
- filtrBufor[filtrZmienna] = filtrSygnal;
- filtrSuma = filtrSuma + filtrBufor[filtrZmienna];
- filtrWynik = filtrSuma / filtrWymiar;
- }
- }
- filtrZmienna++;
- if (filtrZmienna == filtrWymiar) {
- filtrZmienna = 0;
- }
-
- if (filtrWynik < xmin) xmin=filtrWynik;
- if (filtrWynik > xmax) xmax=filtrWynik;
-
+ rollma = filtrWynik;
if (filtr2Zmienna < 0) {
filtr2Wynik = filtr2Sygnal;
@@ -435,132 +208,64 @@
if (filtr2Zmienna == filtr2Wymiar) {
filtr2Zmienna = 0;
}
-
- if (filtr2Wynik < ymin) ymin=filtr2Wynik;
- if (filtr2Wynik > ymax) ymax=filtr2Wynik;
+ pitchma=filtr2Wynik;
+
+}
+
+/*
+void task2()
+{
- if (filtr3Zmienna < 0) {
- filtr3Wynik = filtr3Sygnal;
- filtr3Bufor[filtr3Zmienna + filtr3Wymiar] = filtr3Sygnal;
- filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna + filtr3Wymiar];
- } else {
- if (filtr3Zmienna == filtr3Wymiar - 1) {
- filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
- filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
- filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
- filtr3Wynik = filtr3Suma / filtr3Wymiar;
- } else {
- filtr3Suma = filtr3Suma - filtr3Bufor[filtr3Zmienna];
- filtr3Bufor[filtr3Zmienna] = filtr3Sygnal;
- filtr3Suma = filtr3Suma + filtr3Bufor[filtr3Zmienna];
- filtr3Wynik = filtr3Suma / filtr3Wymiar;
- }
- }
- filtr3Zmienna++;
- if (filtr3Zmienna == filtr3Wymiar) {
- filtr3Zmienna = 0;
- }
-
- if (filtr3Wynik < zmin) zmin=filtr3Wynik;
- if (filtr3Wynik > zmax) zmax=filtr3Wynik;
-
- //sprintf(buff, "%f,%f,%f,%f\n\r", d[4],filtrWynik,ymin,ymax);
- //pc.printf(buff);
}
void task3()
{
- imu.readData(d);
- off.offsetData(d,offsetGyro,o);
- katxzyro = katxzyro + o[0]*dt;
- katyzyro = katyzyro + o[1]*dt;
- katzzyro = katzzyro + o[2]*dt;
- r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
- katx = acos(d[4]/r)-M_PI2;
- katy = acos(d[3]/r)-M_PI2;
-
- kalman_innovate(&pitch_data, katx, o[0]);
- kalman_innovate(&roll_data, -katy, o[1]);
- pitch = pitch_data.x1;
- kalmanpitchzyro = pitch_data.x2;
- kalmanpitchdryf = pitch_data.x3;
- roll = roll_data.x1;
}
*/
+
+
void task_tachometer()
{
TachFlaga=0;
- if(FlagaPom==1){
- PWM4zad+=5;
- M3.pulsewidth_us(PWM4zad);
- }
- wait(0.5f);
- TachFlaga=1;
- //myled2=!myled2;
- RPMtach=impulsA*10; // dla T=3s
- //RPMtach=impulsA*6; //dla T=5s
+
+ //RPMtach=impulsA*10; // dla T=3s
+ RPMtach=impulsA*6; //dla T=5s
//RPMtach=impulsA*3; //dla T=10s
- //RPMtachAktualny=impulsA*3;
- /*
- if (TachFlaga==1){
- RPMtachBuf[iTach]=RPMtachAktualny;
- iTach++;
- if (iTach>3){
- RPMtachWyn=(RPMtachBuf[iTach-1]+RPMtachBuf[iTach-2]+RPMtachBuf[iTach-3])/3;
- RPMtachWynPoprz=RPMtachWyn;
- PWM4zadWyn=PWM4zad;
- TachFlaga=0;
- iTach=0;
- RPMtach=RPMtachAktualny;
- }
- }else{
- if(abs(PWM4zadPoprz-PWM4zad)>0 && abs(RPMtachAktualny-RPMtachWynPoprz)>20){
- TachFlaga=1;
- }
- //RPMtach=RPMtachAktualny;
- PWM4zadPoprz=PWM4zad;
- PWM4zad+=1;
- M4.pulsewidth_us(PWM4zad);
- //if(PWM4zad>14000)PWM4zad=10000;
- }
- sprintf(buff, "%f,%i,%f,%f\n\r", PWM4zad,RPMtachAktualny,PWM4zadWyn,RPMtachWyn);
+
+ sprintf(buff, "%f,%i\n\r", PWM1zad,RPMtach);
pc.printf(buff);
- */
- sprintf(buff, "%f,%i\n\r", PWM4zad,RPMtach);
- pc.printf(buff);
- //iTach++;
- //if(iTach%2>0)PWM4zad+=1;
- //M4.pulsewidth_us(PWM4zad);
- //RPMtachWyn=0;
- //PWM4zadWyn=0;
impulsA=0;
+ TachFlaga=1;
}
+
int main() {
+ zadanie=zad;
+
pc.baud(115200);
bluetooth.baud(19200);
imu.init();
+
+
kalman_init(&pitch_data);
kalman_init(&roll_data);
kalman_init(&pitch_data2);
kalman_init(&roll_data2);
- //event.rise(&rise);
- //event.mode(PullUp);
- //event.enable_irq();
+
sprintf(buff, "Hello: \n\r");
pc.printf(buff);
- off.setOffsets(offsetGyro,&offsetMagneto, bluetooth, imu);
+ off.setOffsets(offsetGyro,&offsetMagneto, pc, imu);
i=1000;
-
+
PWM1zad=10000;
PWM2zad=10000;
PWM3zad=10000;
@@ -575,212 +280,42 @@
M4.period_us(PWM_period);
M4.pulsewidth_us(PWM4zad);
- /*Kp1=10;
- Kp2=8;
- Kp3=0;
- Kd1=8;
- Kd2=16;
- Kd3=5000;
- Ki1=0.1;
- Ki2=0.2;
- Ki3=0;
- Td1=0;
- Td2=0;
- Td3=0;
- Ti1=0;
- Ti2=0;
- Ti3=0;
- T=0.005;*/
- Kp1=12;
- Kp2=12;
- Kp3=0;
- Kd1=2;
- Kd2=2;
- Kd3=0;
- Ki1=0;
- Ki2=0;
- Ki3=0;
- Td1=0;
- Td2=0;
- Td3=0;
- Ti1=0;
- Ti2=0;
- Ti3=0;
- T=0.005;
-
- rollE=0;
-
+ /*rollE=0;
katxzyro = 0;
katyzyro = 0;
- katzzyro = 0;
+ katzzyro = 0;*/
iTach=0;
TachFlaga=0;
-
-a1buf[0][0]= 1.438602213;
-a1buf[0][1]= 1.130064065;
-a1buf[0][2]= 0.958928363;
-a1buf[0][3]= 0.836156086;
-a1buf[0][4]= 0.735165987;
-a1buf[0][5]= 0.619988352;
-a1buf[0][6]= 0.580966803;
-a1buf[0][7]= 0.580966803;
-a1buf[0][8]= 0.580966803;
-a1buf[1][0]= 1.55790332;
-a1buf[1][1]= 1.181479324;
-a1buf[1][2]= 1.001071637;
-a1buf[1][3]= 0.872754805;
-a1buf[1][4]= 0.699592312;
-a1buf[1][5]= 0.644682586;
-a1buf[1][6]= 0.596575422;
-a1buf[1][7]= 0.596575422;
-a1buf[1][8]= 0.596575422;
-a1buf[2][0]= 1.475317414;
-a1buf[2][1]= 1.159883518;
-a1buf[2][2]= 0.965894001;
-a1buf[2][3]= 0.85553873;
-a1buf[2][4]= 0.738311008;
-a1buf[2][5]= 0.627093768;
-a1buf[2][6]= 0.627093768;
-a1buf[2][7]= 0.627093768;
-a1buf[2][8]= 0.627093768;
-a1buf[3][0]= 1.40570763;
-a1buf[3][1]= 1.143366337;
-a1buf[3][2]= 0.953383809;
-a1buf[3][3]= 0.850413512;
-a1buf[3][4]= 0.734304019;
-a1buf[3][5]= 0.634804892;
-a1buf[3][6]= 0.588398369;
-a1buf[3][7]= 0.588305183;
-a1buf[3][8]= 0.588305183;
+
-a2buf[0][0]= -14020.22132;
-a2buf[0][1]= -10629.35935;
-a2buf[0][2]= -8667.80431;
-a2buf[0][3]= -7195.189284;
-a2buf[0][4]= -5935.445545;
-a2buf[0][5]= -4432.964473;
-a2buf[0][6]= -3899.481654;
-a2buf[0][7]= -3899.481654;
-a2buf[0][8]= -3899.481654;
-a2buf[1][0]= -15394.5894;
-a2buf[1][1]= -11261.14735;
-a2buf[1][2]= -9187.344205;
-a2buf[1][3]= -7648.375073;
-a2buf[1][4]= -5478.811881;
-a2buf[1][5]= -4780.460105;
-a2buf[1][6]= -4131.426907;
-a2buf[1][7]= -4131.426907;
-a2buf[1][8]= -4131.426907;
-a2buf[2][0]= -14446.09785;
-a2buf[2][1]= -10984.43215;
-a2buf[2][2]= -8760.145603;
-a2buf[2][3]= -7434.705882;
-a2buf[2][4]= -5971.980198;
-a2buf[2][5]= -4525.428072;
-a2buf[2][6]= -4525.428072;
-a2buf[2][7]= -4525.428072;
-a2buf[2][8]= -4525.428072;
-a2buf[3][0]= -13645.61444;
-a2buf[3][1]= -10761.08911;
-a2buf[3][2]= -8579.586488;
-a2buf[3][3]= -7342.912056;
-a2buf[3][4]= -5896.039604;
-a2buf[3][5]= -4603.83809;
-a2buf[3][6]= -3978.10134;
-a2buf[3][7]= -3976.928363;
-a2buf[3][8]= -3976.928363;
-
-Omegamax[0][0]= 1790;
-Omegamax[0][1]= 2360;
-Omegamax[0][2]= 2830;
-Omegamax[0][3]= 3250;
-Omegamax[0][4]= 3620;
-Omegamax[0][5]= 3950;
-Omegamax[0][6]= 4230;
-Omegamax[0][7]= 4270;
-Omegamax[0][8]= 20000;
-Omegamax[1][0]= 1710;
-Omegamax[1][1]= 2300;
-Omegamax[1][2]= 2820;
-Omegamax[1][3]= 3260;
-Omegamax[1][4]= 3590;
-Omegamax[1][5]= 3920;
-Omegamax[1][6]= 4220;
-Omegamax[1][7]= 4240;
-Omegamax[1][8]= 20000;
-Omegamax[2][0]= 1760;
-Omegamax[2][1]= 2360;
-Omegamax[2][2]= 2810;
-Omegamax[2][3]= 3250;
-Omegamax[2][4]= 3630;
-Omegamax[2][5]= 3940;
-Omegamax[2][6]= 4110;
-Omegamax[2][7]= 4110;
-Omegamax[2][8]= 20000;
-Omegamax[3][0]= 1790;
-Omegamax[3][1]= 2380;
-Omegamax[3][2]= 2880;
-Omegamax[3][3]= 3270;
-Omegamax[3][4]= 3650;
-Omegamax[3][5]= 3970;
-Omegamax[3][6]= 4250;
-Omegamax[3][7]= 4300;
-Omegamax[3][8]= 20000;
-
- a1[0]=a1buf[0][0];
- a2[0]=a2buf[0][0];
- a1[1]=a1buf[1][0];
- a2[1]=a2buf[1][0];
- a1[2]=a1buf[2][0];
- a2[2]=a2buf[2][0];
- a1[3]=a1buf[3][0];
- a2[3]=a2buf[3][0];
-
- triger1.attach(&task1, 0.005);
- //triger2.attach(&task2, 0.005);
- //triger3.attach(&task3, 0.005);
- //tachometer.attach(&task_tachometer, 3.5);
+ switch (zadanie){
+ case 1:
+ triger1.attach(&task1, 0.005);
+ break;
+
+ case 2:
+ event.rise(&rise);
+ event.mode(PullUp);
+ event.enable_irq();
+ tachometer.attach(&task_tachometer, 5);
+ break;
+ };
while(1) {
+//WYSYLANIE DANYCH PO PC UART
- //myled2 = !myled2;
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //sprintf(buff, "%f,%f,%f\n\r", d[0],d[1],d[2]);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //sprintf(buff, "%f,%f,%f,%f,%f\n\r", valPWM4,RPMtach,valPWM3,valPWM2,valPWM1);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //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);
- //pc.printf(buff);
- //myled2 = !myled2;
- //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]);
- //pc.printf(buff);
-
-
-
- sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", roll*180/M_PI,valPWM1,valPWM2,valPWM3,valPWM4,Kp1,Kd1,Ki1);
- pc.printf(buff);
+ switch (zadanie){
+ case 1:
+ sprintf(buff, "0,%f,%f,%f,%f\n\r", -katy*180/M_PI, katzyro*180/M_PI, roll*180/M_PI);
+ pc.printf(buff);
+ break;
+
+ case 2:
+ break;
+ };
if(pc.readable()){
@@ -788,166 +323,48 @@
switch (znak){
case 'r':
- sprintf(buff, "Resetuje zmienne %c\n\r",znak);
- pc.printf(buff);
- xmin=0;
- ymin=0;
- zmin=0;
- xmax=0;
- ymax=0;
- zmax=0;
break;
case 't':
- PWM4zad++;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'g':
- PWM4zad--;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'y':
- PWM4zad+=10;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'h':
- PWM4zad-=10;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'u':
- PWM4zad+=100;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'j':
- PWM4zad-=100;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 's':
- PWM4zad=10000;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'b':
- PWM4zad=10000;
- M3.pulsewidth_us(PWM4zad);
- break;
- case 'z':
- FlagaPom=1;
- break;
- case 'x':
- FlagaPom=0;
+ PWM1zad+=50;
+ if(PWM1zad>=20000){
+ PWM1zad=20000;
+ }
+ M1.pulsewidth_us(PWM1zad);
break;
- /*case 'u':
- PWM1zad+=10;
+ case 'g':
+ PWM1zad-=50;
+ if(PWM1zad<10000){
+ PWM1zad=10000;
+ }
M1.pulsewidth_us(PWM1zad);
break;
- case 'j':
- PWM1zad-=10;
- M1.pulsewidth_us(PWM1zad);
- break;*/
- }
- znak=0;
- }
-
-
- if(bluetooth.readable()){
-
- znak2=bluetooth.getc();
-
- switch (znak2){
+ case 'y':
+ break;
+ case 'h':
+ break;
- case 'a':
- for(jster=0;jster<4;jster++){
- Omegazad[jster]-=50;
- }
- flagaster=1;
-
- /*T1zad-=0.05;
- T2zad-=0.05;
- T3zad-=0.05;
- T4zad-=0.05;
- PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
- PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
- PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
- PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
+ case 'u':
+ break;
- /*
- //PWM3zad-=50;
- PWM4zad-=50;
- //PWM2zad=1.045*(PWM4zad-10000)+10000;
- //PWM1zad=1.026*(PWM3zad-10000)+10000;
+ case 'j':
+ break;
- //PWM1zad=1.026*(PWM3zad-10000)+10000;
- //PWM1zad-=50;
- if(PWM1zad<10000){
- PWM1zad=10000;
- PWM2zad=10000;
- PWM3zad=10000;
- PWM4zad=10000;
- }
- //ustawianie
- M1.pulsewidth_us(PWM1zad);
- M2.pulsewidth_us(PWM2zad);
- M3.pulsewidth_us(PWM3zad);
- M4.pulsewidth_us(PWM4zad);
- */
- znak2=0;
-
+ case 's':
break;
case 'b':
- for(jster=0;jster<4;jster++){
- Omegazad[jster]+=50;
- }
- flagaster=1;
- /*T1zad+=0.05;
- T2zad+=0.05;
- T3zad+=0.05;
- T4zad+=0.05;
- PWM1zad=1287.588322*T1zad-(-431.2892625)+10000;
- PWM2zad=1245.207965*T2zad-(-600.7566265)+10000;
- PWM3zad=1295.740776*T3zad-(-514.6040317)+10000;
- PWM4zad=1269.412072*T4zad-(-568.7123367)+10000;*/
- /*
- //PWM3zad+=50;
- PWM4zad+=50;
- //PWM2zad=1.045*(PWM4zad-10000)+10000;
- //PWM3zad+=50;
- //PWM1zad=1.026*(PWM3zad-10000)+10000;
+ break;
- //PWM1zad+=50;
- if(PWM1zad>=20000){
- PWM1zad=20000;
- PWM2zad=20000;
- PWM3zad=20000;
- PWM4zad=20000;
- }
- //ustawianie
- M1.pulsewidth_us(PWM1zad);
- M2.pulsewidth_us(PWM2zad);
- M3.pulsewidth_us(PWM3zad);
- M4.pulsewidth_us(PWM4zad);
- */
- znak2=0;
+ case 'z':
break;
case 'x':
- //sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
- //pc.printf(buff);
- //wait(1.0f);
- for(jster=0;jster<4;jster++){
- Omegazad[jster]=0;
- }
PWM1zad=10000;
- PWM2zad=10000;
- PWM3zad=10000;
- PWM4zad=10000;
M1.pulsewidth_us(PWM1zad);
- M2.pulsewidth_us(PWM2zad);
- M3.pulsewidth_us(PWM3zad);
- M4.pulsewidth_us(PWM4zad);
sprintf(buff,"Odlacz silniki\n\r");
pc.printf(buff);
wait(1.0f);
@@ -970,66 +387,116 @@
pc.printf(buff);
break;
+ case 0x20:
+ PWM1zad=10000;
+ M1.pulsewidth_us(PWM1zad);
+ sprintf(buff,"Odlacz silniki\n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"5 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"4 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"3 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"2 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"1 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"GO! \n\r");
+ pc.printf(buff);
+ break;
+
+ }
+
+ znak=0;
+ }
+
+
+ if(bluetooth.readable()){
+
+ znak2=bluetooth.getc();
+
+ switch (znak2){
+
+ case 'a':
+ PWM1zad-=50;
+ flagaster=1;
+ znak2=0;
+ break;
+
+ case 'b':
+ PWM1zad+=50;
+ flagaster=1;
+ znak2=0;
+ break;
+
+ case 'x':
+ PWM1zad=10000;
+ M1.pulsewidth_us(PWM1zad);
+
+ sprintf(buff,"Odlacz silniki\n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"5 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"4 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"3 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"2 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"1 \n\r");
+ pc.printf(buff);
+ wait(1.0f);
+ sprintf(buff,"GO! \n\r");
+ pc.printf(buff);
+ break;
+
+
case 'c':
- Kd1-=0.5;
- Kd2-=0.5;
+
break;
case 'd':
- Kd1+=0.5;
- Kd2+=0.5;
+
break;
case 'e':
- Kp1-=0.5;
- Kp2-=0.5;
+
break;
case 'f':
- Kp1+=0.5;
- Kp2+=0.5;
+
break;
case 'g':
- Ki1-=0.1;
- Ki2-=0.1;
+
break;
case 'h':
- Ki1+=0.1;
- Ki2+=0.1;
+
break;
}
if(flagaster==1){
- for(int jster=0;jster<4;jster++){
- ister=-1;
- do{
- ister++;
- }while(Omegazad[jster] > Omegamax[jster][ister]);
- a1[jster]=a1buf[jster][ister];
- a2[jster]=a2buf[jster][ister];
- }
- PWM1zad=(Omegazad[0]-a2[0])/a1[0];
- PWM2zad=(Omegazad[1]-a2[1])/a1[1];
- PWM3zad=(Omegazad[2]-a2[2])/a1[2];
- PWM4zad=(Omegazad[3]-a2[3])/a1[3];
if(PWM1zad<10000){
PWM1zad=10000;
- }
- if(PWM2zad<10000){
- PWM2zad=10000;
- }
- if(PWM3zad<10000){
- PWM3zad=10000;
- }
- if(PWM4zad<10000){
- PWM4zad=10000;
- }
- M1.pulsewidth_us(PWM1zad);
- M2.pulsewidth_us(PWM2zad);
- M3.pulsewidth_us(PWM3zad);
- M4.pulsewidth_us(PWM4zad);
-
+ }
+
+ if(PWM1zad>=20000){
+ PWM1zad=20000;
+ }
+
+ M1.pulsewidth_us(PWM1zad);
flagaster=0;
}