File content as of revision 12:e955cc086c42:

#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
#define filtrWymiar 5
#define filtrSygnal roll
#define filtr2Wymiar 5
#define filtr2Sygnal pitch
#define filtr3Wymiar 100
#define filtr3Sygnal d[5]



#define zad 1


//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;

InterruptIn event(PTB23);

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

Ticker triger1; //przerwanie filtracji
//Ticker triger2; //kalibracja accelero
//Ticker triger3; //kalibracja zyro
Ticker tachometer;

float d[9];
double D[9];
float o[3];
float O[3];
char buff[160];
float r,katx,katy,katz,katzyro;
float rbut,katxbut,katybut;
float pitch, roll, yaw;
float pitch2, roll2;
float pitchma, rollma;

double i;
float offsetGyro[3];
float offsetMagneto;
float yawOff;
char odczyt[20];


char znak;
char znak2;

int impulsA;
double PWM1zad;
double PWM2zad;
double PWM3zad;
double PWM4zad;


//zmienne tymczasowe
float katxzyro,katyzyro,katzzyro;
float fkompl;

float filtrBufor[filtrWymiar];
int   filtrZmienna=0;
float filtrSuma=0;
float filtrWynik;
float filtr2Bufor[filtrWymiar];
int   filtr2Zmienna=0;
float filtr2Suma=0;
float filtr2Wynik;
float filtr3Bufor[filtrWymiar];
int   filtr3Zmienna=0;
float filtr3Suma=0;
float filtr3Wynik;

float kalmanpitchzyro;
float kalmanpitchzyrobut;
float kalmanrollzyro;
float kalmanrollzyrobut;
float kalmanpitchdryf;

int RPMtach;
int zadanie;
/*
float PWM4zadBuf[5];
float PWM4zadPoprz;
float PWM4zadWyn;
float RPMtachBuf[5];
int RPMtachPoprz;
int RPMtachAktualny;
float RPMtachWyn;
int RPMtachWynPoprz;
*/

int iTach;
bool TachFlaga;
bool FlagaPom;

bool flagaster;

void rise(void)
{
    if(TachFlaga==1)impulsA++;
}

void task1()
{
    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;
       
    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;
    }
 
    if(yawOff > 2 * M_PI){
    yawOff-= 2 * M_PI;
    }*/

    //Filtr Kalmana
    kalman_innovate(&pitch_data, katx, o[0]);
    kalman_innovate(&roll_data, -katy, o[1]);
    pitch = -pitch_data.x1;
    kalmanpitchzyro = pitch_data.x2;
    roll = roll_data.x1;
    kalmanrollzyro = roll_data.x2;
  

    //moving avarage
        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;
        }
    rollma = 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;
        }
    pitchma=filtr2Wynik;
            
}

/*
void task2()
{
    
}


void task3()
{
    
}
*/



void task_tachometer()
{
    
    TachFlaga=0;
    
    //RPMtach=impulsA*10; // dla T=3s
    RPMtach=impulsA*6;   //dla T=5s
    //RPMtach=impulsA*3;   //dla T=10s
    
    sprintf(buff, "%f,%i\n\r", PWM1zad,RPMtach);
    pc.printf(buff);
    
    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);
    

    sprintf(buff, "Hello: \n\r");
    pc.printf(buff);
    
    off.setOffsets(offsetGyro,&offsetMagneto, pc, imu);
    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);
    
    /*rollE=0;
    katxzyro = 0;
    katyzyro = 0;
    katzzyro = 0;*/
    
    iTach=0;
    TachFlaga=0;


    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 
    
    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()){
        znak=pc.getc();
        switch (znak){
            
            case 'r':
            break;
            
            case 't':
            PWM1zad+=50;
            if(PWM1zad>=20000){
            PWM1zad=20000;
            }
            M1.pulsewidth_us(PWM1zad);
            break;
            
            case 'g':
            PWM1zad-=50;
            if(PWM1zad<10000){
            PWM1zad=10000;
            }
            M1.pulsewidth_us(PWM1zad);
            break;
            
            case 'y':
            break;
            
            case 'h':
            break;
            
            case 'u':
            break;
            
            case 'j':
            break;
            
            case 's':
            break;
            
            case 'b':
            break;
            
            case 'z':
            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 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':

            break;
            case 'd':

            break;    
            case 'e':

            break;
            case 'f':

            break;   
            case 'g':

            break;  
            case 'h':

            break;                    
        }    
        
        
        if(flagaster==1){


        
        if(PWM1zad<10000){
        PWM1zad=10000;
        }

        if(PWM1zad>=20000){
        PWM1zad=20000;
        }

        M1.pulsewidth_us(PWM1zad);      
        flagaster=0;
        }
        
        }//switch
    //myled2 = !myled2;
    }//while(1)
}//main