File content as of revision 14:a75b20f9cc24:

#include "mbed.h"
#include "stdio.h"
#define PI 3.14159


int16_t high, low;

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
                                    
                                    

                                    
static int n1, n2, n3, n4;

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

float read_x_acc ()
{ 
    float xd = aktuell_acc_x();
    xd/=16384;
    if(xd<-1.0){return -1.0;}
    if(xd>1.0){return 1.0;}
    else return xd;
}

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

float read_y_acc ()
{ 
    float yd = aktuell_acc_y();
    xd/=16384;
    if(yd<-1.0){return -1.0;}
    if(yd>1.0){return 1.0;}
    else return yd;
}

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

float read_z_acc ()
{ 
    float zd = aktuell_acc_z();
    zd/=16384;
    if(zd<-1.0){return -1.0;}
    if(zd>1.0){return 1.0;}
    else return zd;
}

double pitch ()      
{
    double x,z; 
    x=read_x_acc(); z=read_z_acc();
    double pitch = atan2(-x,z)*180/PI;
    return pitch;
}

double roll ()
{
    double x,y,z; 
    y=read_y_acc(); x=pow(read_x_acc(),2); z=pow(read_z_acc(),2);
    double roll = atan2(y,sqrt(x+z))*180/PI;
    return roll;
}


/**********/
/*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);
    }
}