File content as of revision 7:2ba30a0cdc16:

#include "mbed.h"
#include "FastPWM.h"
#include "kalman.h"
#include "Offsets.h"
#include "stdio.h"
#include "IMU.h"

#define PWM_period 2500
#define M_PI 3.141592
#define M_PI2 1.570796
#define dt 0.005


//kalman
// Structs for containing filter data
kalman_data pitch_data;
kalman_data roll_data;

kalman_data pitch_data2;
kalman_data roll_data2;

DigitalOut myled(PTA2);
DigitalOut myled2(PTA1);
Serial pc(USBTX, USBRX);
Serial bluetooth(D1, D0);
IMU imu(PTE25,PTE24);
Offsets off;

FastPWM M1(D10);
FastPWM M2(D11);
FastPWM M3(D12);
FastPWM M4(D13);


Ticker triger1; //przerwanie filtracji
//Ticker triger2; //przerwanie wysyłania danych

float d[9];
double D[9];
float o[3];
float O[3];
char buff[160];
float r,katx,katy;
float rbut,katxbut,katybut;
float pitch, roll;
float pitch2, roll2;
float pitchE, rollE;
double i;
float offsetGyro[3];
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;

double PWM1zad;
double PWM2zad;
double PWM3zad;
double PWM4zad;
double valPWM1;
double valPWM2;
double valPWM3;
double valPWM4;

void task1()
{
    //myled = !myled;
    imu.readData(d);
    imu.filterData(d, D);
    off.offsetData(d,offsetGyro,o);
    off.offsetData2(D,offsetGyro,O);

    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;
    
    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;
    
    
    //Filtr Kalmana
    kalman_innovate(&pitch_data, katx, o[0]);
    kalman_innovate(&roll_data, -katy, o[1]);
    pitch = pitch_data.x1;
    roll = roll_data.x1;
    
    //Filtr Kalmana butterworth 2nd
    kalman_innovate(&pitch_data2, katxbut, O[0]);
    kalman_innovate(&roll_data2, -katybut, O[1]);
    pitch2 = pitch_data2.x1;
    roll2 = roll_data2.x1;
    
    //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  **********************
    // 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/b*PWM1zad+C1/b)-U2/2bl+U3/4d
    //omega2^2=(B/b*PWM2zad+C/b)+U1/2bl-U3/4d
    //omega3^2=(B3/b*PWM3zad+C3/b)+U2/2bl+U3/4d
    //omega4^2=(B/b*PWM4zad+C/b)-U1/2bl-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=0.000015
    //B1=0.000776646
    //C1=-0.334958973
    //l=0.3
    //d=0.000033
    //2bl=0.0000092780
    //4d=0.000132
    //B1/b=50.22498189
    //C1/b=-21611.4954
    //b/B1=0.01991041
    //C1/B1=-431.2892625
    
    //B3/b=49.90897978
    //C3/b=-25683.36221
    //b/B3=0.020036474
    //C3/B3=-514.6040317



    
    pitchE=pitchE+(0-pitch2);
    if(pitchE>3) pitchE=3;
    Kd1=Kp1*Td1/T;
    Ki1=Kp1*T/Ti1;
    if(Ti1==0)Ki1=0;
    
    rollE=rollE+(0-roll2);
    if(rollE>3) rollE=3;
    Kd2=Kp2*Td2/T;
    Ki2=Kp2*T/Ti2;
    if(Ti2==0)Ki2=0;
    
    /* yawE=yawE+(0-roll2);
    if(yawE>3) yawE=3;*/
    Kd3=Kp3*Td3/T;
    Ki3=Kp3*T/Ti3;
    if(Ti3==0)Ki3=0;
    
    U1 = 0.0173*(Kp1*(0-pitch2)+Kd1*(0-O[0])+Ki1*pitchE);
    U2 = 0.0169*(Kp2*(0-roll2)+Kp2*Td2/T*(0-O[1])+Kp2*T/Ti2*rollE);
    U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2]));//+Ki3*rollE);
        
    Om1 = 50.22498189*PWM1zad+(-21611.4954)-U2/0.0000092780 + U3/0.000132 ;             //kwadraty
    Om2 = 50.22498189*PWM2zad+(-21611.4954)+U1/0.0000092780 - U3/0.000132 ; 
    Om3 = 49.90897978*PWM3zad+(-25683.36221)+U2/0.0000092780 + U3/0.000132 ;
    Om4 = 50.22498189*PWM4zad+(-21611.4954)-U1/0.0000092780 - U3/0.000132 ;
    
    wyp1=0.01991041*Om1+(-431.2892625);
    wyp2=0.01991041*Om2+(-431.2892625);
    wyp3=0.020036474*Om3+(-514.6040317);
    wyp4=0.01991041*Om4+(-431.2892625);
        
    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>10000 & wyp2<20000) valPWM2=(int)wyp2;

    if(wyp3<=10001 | wyp3>40001) valPWM3=10000;
    if(wyp3>=20000 & wyp3<40000) valPWM3=20000;
    if(wyp3>10100 & wyp3<20000) valPWM3=(int)wyp3;

    if(wyp4<=10001 & wyp4>40001) valPWM4=10000;
    if(wyp4>=20000 & wyp4<40000) valPWM4=20000;
    if(wyp4>10000 & 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;*/
}

        


int main() {
    
    pc.baud(115200);
    bluetooth.baud(19200);
    imu.init();
    kalman_init(&pitch_data);
    kalman_init(&roll_data);
    kalman_init(&pitch_data2);
    kalman_init(&roll_data2);
    
    sprintf(buff, "Hello: \n\r");
    pc.printf(buff);
    
    off.setOffsets(offsetGyro, pc, imu);
    

    
    triger1.attach(&task1, 0.005);
    //triger2.attach(&task2, 0.005);
    i=1000;
    
    
    PWM1zad=10000;
    PWM2zad=10000;
    PWM3zad=10000;
    PWM4zad=10000;
    
    M1.period_us(PWM_period);
    M1.pulsewidth_us(PWM1zad);
    M2.period_us(PWM_period);
    M2.pulsewidth_us(PWM2zad);
    M3.period_us(PWM_period);
    M3.pulsewidth_us(PWM3zad);
    M4.period_us(PWM_period);
    M4.pulsewidth_us(PWM4zad);
    
    Kp1=0;
    Kp2=0;
    Kp3=0;
    Td1=0;
    Td2=0;
    Td3=0;
    Ti1=0;
    Ti2=0;
    Ti3=0;
    T=0.005;
    
    
    while(1) {
    
    //myled2 = !myled2;
    //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);
    //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);
    //pc.printf(buff);
    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);
    pc.printf(buff);
    //sprintf(buff, "%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI);
    //pc.printf(buff);
    //sprintf(buff, "%f,%f,%f,%f\n\r", valPWM1, valPWM2, valPWM3, valPWM4);
    //pc.printf(buff);
    
    //myled2 = !myled2;
    
         
    
    if(pc.readable()){
        znak=pc.getc();
        switch (znak){
            
            case 'p':
            sprintf(buff, "odczytany znak: %c\n\r",znak);
            pc.printf(buff);
            break;
            /*case 'u':
            PWM1zad+=10;
            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 'a':
            PWM1zad-=50;
            PWM2zad-=50;
            PWM3zad-=50;
            PWM4zad-=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;
            break;
            
            case 'b':
            PWM1zad+=50;
            PWM2zad+=50;
            PWM3zad+=50;
            PWM4zad+=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;
            break;
            
            case 'x':
            sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Td2);
            pc.printf(buff);
            wait(1.0f);
            Kp1=0;
            Td1=0;
            Kp2=0;
            Td2=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);
            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':
            Td1-=0.001;
            Td2-=0.001;
            break;
            case 'd':
            Td1+=0.001;
            Td2+=0.001;
            break;    
            case 'e':
            Kp1-=0.5;
            Kp2-=0.5;
            break;
            case 'f':
            Kp1+=0.5;
            Kp2+=0.5;
            break;                     
        }        
        }
    //myled2 = !myled2;
    }
}