File content as of revision 15:742683a8efda:

#include "mbed.h"
#include "stdio.h"





extern Serial pc(SERIAL_TX, SERIAL_RX);
extern SPI spi(PE_6,PE_5,PE_2);         //mosi,miso,sclk
extern DigitalOut ncs(PE_4);            //ssel

extern AnalogIn potis_1 (PF_3);
extern AnalogIn potis_2 (PF_10);
extern AnalogIn potis_3 (PF_4);
extern AnalogIn potis_4 (PF_5);

extern PwmOut Motor1 (PC_8);        //  Schwarz  QBRAIN: rot
extern PwmOut Motor2 (PC_9);        //  Weiß             orange
extern PwmOut Motor3 (PC_6);        //  Grau             weiß
extern PwmOut Motor4 (PB_9);        //  Blau             braun
                                    //  Gelb und Orange Vcc +5V
                                    //  Gnd Rot
                                    
int n1, n2, n3, n4;
int16_t high, low;

/**********************/
/*Initialisieren**Gyro*/
/**********************/
void initialisierung_gyro()
{
    spi.format(8,0);
    spi.frequency(1000000);
    
    ncs = 0;
    spi.write(0x6B);     // Register 107
    spi.write(0x80);     //Reset // Standby off
    ncs = 1;               
    wait_ms(1000); 
    
        
    ncs=0;
    spi.write(0x1A);     //CONFIG  write // DLPF_CFG // Register 26
    spi.write(0x06);     //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
    ncs = 1;                
    wait_ms(1);
        
    ncs=0;
    spi.write(0x1B);     //Gyro_CONFIG  write  
    spi.write(0x18);     //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
    ncs = 1;                
    wait_ms(1); 

    ncs = 0;
    spi.write(0x17);     // Register 23
    spi.write(0x00);     // Offset High Byte Z
    ncs = 1;               
    wait_ms(1); 
    
    ncs = 0;
    spi.write(0x18);     // Register 24
    spi.write(0x17);     // Offset Low Byte Z
    ncs = 1;               
    wait_ms(1000); 
}

/**********************/
/*Initialisieren Acc  */
/**********************/

int16_t initialisierung_acc ()
{
    int i,faktor = 0x00; 
    for(i=0;i<=2;i++)
    {
    ncs=0;
    spi.write(0x1c);     //ACC_CONFIG  write  
    spi.write(faktor);  //Skalierung    00=2g;08=4g;10=8g;18=16g
    ncs=1;               //Teilung 16384;8192;4096;2048
    wait_us(0.1);      
    
    ncs=0;
    spi.write(0x1d);     //ACC_CONFIG_2  08=460Hz;09=184Hz;0a=92Hz
    spi.write(0x0e);      //TP-Filter     0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz
    ncs=1;
    wait_us(0.1);
    }
    switch (faktor)
    {
        case 0x00: faktor = 16384; break;
        case 0x08: faktor =  8192; break;
        case 0x10: faktor =  4096; break;
        case 0x18: faktor =  2048; break;
    }
    return faktor;
}


/*******************/
/*Messen Sensor roh*/
/*******************/

void aktuell_roh(int16_t *z_g, int16_t *x_g, int16_t *y_g, int16_t *z_a, int16_t *x_a, int16_t *y_a)
{  
    ncs = 0;
    spi.write(0xc7);                    
    high = spi.write(0x0);
    ncs = 1;
    wait_us(0.1); 
    ncs = 0;
    spi.write(0xc8);                    
    low = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);         
    (*z_g) = low | (high << 8);    
    
    ncs = 0;
    spi.write(0xc3);                    
    high = spi.write(0x0);
    ncs = 1;
    wait_us(1); 
    ncs = 0;
    spi.write(0xc4);                    
    low = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);           
    (*x_g) = low | (high << 8);

    ncs = 0;
    spi.write(0xc5);                    
    high = spi.write(0x0);
    ncs = 1;
    wait_us(0.1); 
    ncs = 0;
    spi.write(0xc6);                    
    low = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);
    (*y_g) = low | (high << 8);     
             
    ncs=0;
    spi.write(0xbb);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
    ncs=0;
    spi.write(0xbc);            
    low = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
    (*x_a) = low | (high << 8); 

    ncs=0;
    spi.write(0xbd);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
    ncs=0;
    spi.write(0xbe);            
    low = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
    (*y_a) = low | (high << 8); 

    ncs=0;
    spi.write(0xbf);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);  
    ncs=0;
    spi.write(0xc0);            
    low = spi.write(0x0);
    ncs=1;  
    wait_us(0.1);
    (*z_a) = low | (high << 8); 
}


/***************/
/*Messen Gyro Z*/
/***************/

int16_t aktuell_gyro_z()
{  
    ncs = 0;
    spi.write(0xc7);                    //Z_OUT_H
    high = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);
       
    ncs = 0;
    spi.write(0xc8);                    //Z_OUT_L
    low = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);         
    return (low | high << 8);                  
}

/***************/
/*Messen Gyro X*/
/***************/

int16_t aktuell_gyro_x()
{   
    ncs = 0;
    spi.write(0xc3);                    //Z_OUT_H
    high = spi.write(0x0);
    ncs = 1;
    wait_us(1);
       
    ncs = 0;
    spi.write(0xc4);                    //Z_OUT_L
    low = spi.write(0x0);
    ncs = 1;
       wait_us(0.1);           
    return low | high << 8;;                  
}

/***************/
/*Messen Gyro Y*/
/***************/

int16_t aktuell_gyro_y()
{
    ncs = 0;
    spi.write(0xc5);                    //Z_OUT_H
    high = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);
       
    ncs = 0;
    spi.write(0xc6);                    //Z_OUT_L
    low = spi.write(0x0);
    ncs = 1;
    wait_us(0.1);
    return low | high << 8;                  
}

/************/
/*Messen Acc*/
/************/

int16_t aktuell_acc_x ()
{   
    ncs=0;
    spi.write(0xbb);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
   
    ncs=0;
    spi.write(0xbc);            
    low = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
    return low | high<<8; 
}    

int16_t aktuell_acc_y ()
{   
    ncs=0;
    spi.write(0xbd);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
   
    ncs=0;
    spi.write(0xbe);            
    low = spi.write(0x0);
    ncs=1;
        wait_us(0.1);
    return low | high<<8; 
} 

int16_t aktuell_acc_z ()
{   
    ncs=0;
    spi.write(0xbf);            
    high = spi.write(0x0);
    ncs=1;
    wait_us(0.1);
   
    ncs=0;
    spi.write(0xc0);            
    low = spi.write(0x0);
    ncs=1;  
        wait_us(0.1);
    return low | high<<8; 
} 


/**********/
/*Anlernen*/
/**********/
void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4)
{
    int n1 = n2 = n3 = n4 = 1900;
    Motor1->pulsewidth_us(n1);
    Motor2->pulsewidth_us(n2);
    Motor3->pulsewidth_us(n3);
    Motor4->pulsewidth_us(n4);
    pc.printf("Nach einem langem PiepTon  Taste1 betaetigen\n\r");
    pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
    while (!*taster4) 
    {
        if (*taster1) 
        {
            n1 = n2 =n3 = n4 = 700;
        }
        if (*taster2) 
        {
            n1 = n2 = n3 = n4 =1900;
        }
        Motor1->pulsewidth_us(n1);
        Motor2->pulsewidth_us(n2);
        Motor3->pulsewidth_us(n3);
        Motor4->pulsewidth_us(n4);
        pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
    }
}

/**********/
/*Rauschen*/
/**********/
void viberationen(AnalogOut *rauschen, PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster4)
{
    (*rauschen) = 1;
    int n1 = n2 = n3 = n4 = 700;
    Motor1->pulsewidth_us(n1);
    Motor2->pulsewidth_us(n2);
    Motor3->pulsewidth_us(n3);
    Motor4->pulsewidth_us(n4);
    wait_ms(10000);
    while(!*taster4)
    {
        for(int i = 1; i <= 1000; i++)
        {
        float k = aktuell_acc_x()*aktuell_acc_x();
        k = sqrt(k) * 0.0000438596491;
        pc.printf("Winkel:%2.5f\n\r",k);
        (*rauschen) = k;
        wait_ms(10);
        }
        n1 = n2 = n3 = n4 = 1400;
        Motor1->pulsewidth_us(n1);
        Motor2->pulsewidth_us(n2);
        Motor3->pulsewidth_us(n3);
        Motor4->pulsewidth_us(n4);
        for(int i = 1; i <= 3000; i++)
        {
        float k = aktuell_acc_x() * aktuell_acc_x();
        k = sqrt(k) * 0.0000438596491;
        (*rauschen) = k;
        wait_ms(10);
        }
        (*rauschen) = 0;
        wait_ms(100000);
    }
}              
/****************/
/*Offset****Gyro*/
/****************/   
void offset_gyro(float *z_off, float *x_off, float *y_off)
{
    float z_off1, z_off2;
    float x_off1, x_off2;
    float y_off1, y_off2;
    float i;
    z_off2 = 1;
    x_off2 = 1;
    y_off2 = 1;
    z_off2 = 1;
    do
    { 
        for(i = 1; i <= 5000; i++)
        {   
            if ((z_off2 > 0.05) || (z_off2 < -0.05))
            {
                z_off1 += aktuell_gyro_z();
            }
            if ((y_off2 > 0.05) || (y_off2 < -0.05))
            {
                y_off1 += aktuell_gyro_y();
            }
            if ((x_off2 > 0.05) || (x_off2 < -0.05))
            {
                x_off1 += aktuell_gyro_x();
            }                                                         
                wait_ms(1);
        }
        if ((z_off2 > 0.05) || (z_off2 < -0.05))
        {
            z_off1 /= i;
        }
        if ((y_off2 > 0.05) || (y_off2 < -0.05))
        {
            y_off1 /= i;
        }
        if ((x_off2 > 0.05) || (x_off2 < -0.05))
        {
            x_off1 /= i;
        }
        for(i = 1; i <= 5000; i++)
        { 
            if ((z_off2 > 0.05) || (z_off2 < -0.05))
            {
                z_off2 += aktuell_gyro_z() - z_off1;
            }
            if ((y_off2 > 0.05) || (y_off2 < -0.05))
            {
                y_off2 += aktuell_gyro_y() - y_off1;
            }
            if ((x_off2 > 0.05) || (x_off2 < -0.05))
            {
                x_off2 += aktuell_gyro_x() - x_off1;
            }         
            wait_ms(1);
        }   
        if ((z_off2 > 0.05) || (z_off2 < -0.05))
        {
            z_off2 /= i;
        }
        if ((y_off2 > 0.05) || (y_off2 < -0.05))
        {
            y_off2 /= i;
        }
        if ((x_off2 > 0.05) || (x_off2 < -0.05))
        {
            x_off2 /= i;
        }
    } while(((z_off2 > 0.05) || (z_off2 < -0.05)) || ((y_off2 > 0.05) || (y_off2 < -0.05)) || ((x_off2 > 0.05) || (x_off2 < -0.05)));
    (*z_off) = z_off1 + z_off2;
    (*y_off) = y_off1 + y_off2;
    (*x_off) = x_off1 + x_off2;
}
 
/****************/
/*Drift*****Gyro*/
/****************/  
void drift_gyro(float *drift_z, float *drift_x, float *drift_y, Timer *timer, Timer *timer2, double *gain_g, double *roll_g, double *pitch_g, float *z_off, float *x_off, float *y_off)
{
    timer->stop();
    timer2->stop();
    timer->reset();
    timer2->reset();
    timer->start();
    timer2->start();     
    while(timer2->read_ms() <= 60000)
    {   
        uint32_t zeit = timer->read_us(); 
        timer->reset();             
        (*gain_g) = (*gain_g) + ((aktuell_gyro_z() - (*z_off)) * zeit * 0.000001 * 1/16.4);
        (*pitch_g) = (*pitch_g) + ((aktuell_gyro_y() - (*y_off)) * zeit * 0.000001 * 1/16.4);
        (*roll_g) = (*roll_g) + ((aktuell_gyro_x() - (*x_off)) * zeit * 0.000001 * 1/16.4);
        
    }
    (*drift_z) = (*gain_g)/30000;    //Drift alle 4ms
    (*drift_y) = (*pitch_g)/30000;    //Drift alle 4ms
    (*drift_x) = (*roll_g)/30000;    //Drift alle 4ms
}