File content as of revision 3:6e4ca952e920:

#include "mbed.h"
#include "MPU9250.h"
#include "Servo.h"
#include "DebounceIn.h"

AnalogIn   pot(A0);
PwmOut myservo(D6);
DigitalOut LedNorma(D2), LedVolante(D3), LedAsa(D4), LedAcelera(D5), myled1(LED1);
DebounceIn mybutton(USER_BUTTON);

float sum = 0;
uint32_t sumCount = 0;

float axf=0, ayf=0, azf=0, soma=0, naxf=0, norma=0, norma2=0, g=0, pr[10];
int Flag_botao=0, Flag_naxf=0,Flag_Norma=0, Flag_axf=0, Flag_DRS=0, Flag_vs=0, Flag_Volante=1, i;

char serial;
bool inicio = true;

Timer t;

MPU9250 mpu9250;

Serial pc(USBTX, USBRX); // tx, rx

volatile bool newData = false;

InterruptIn isrPin(D12);   //k64 D12  dragon PD_0

void mpuisr()
    {
     newData=true;
    }

int main(){
    
    pc.baud(9600);

    //Set up I2C
    i2c.frequency(400000); 
    t.start();
    isrPin.rise(&mpuisr);
    
    myservo.period_ms(20);
    myservo.pulsewidth_ms(1.5);
                 
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // numero registro I2C MPU9250

         if (whoami == 0x73){
             wait(1);
             mpu9250.resetMPU9250(); // Reset registradores
             mpu9250.calibrateMPU9250(gyroBias, accelBias); // calibração giroscopio e acelerometro
             wait(2);
             mpu9250.initMPU9250();
             mpu9250.initAK8963(magCalibration); // Calibração magnetometro
             wait(2);
            }
               else{
                while(1) ; // loop de não comunicação
                 }

             mpu9250.getAres(); // valor setado de sensibilidade
             mpu9250.getGres(); 
             mpu9250.getMres();
             magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
             magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
             magbias[2] = +125.;  // User environmental x-axis correction in milliGauss

        while(1) {  
                static int readycnt=0;
#if USE_ISR
                if(newData) {
                        newData=false;
                        mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS);  //? need this with ISR
#else
                                if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
                #endif
                                        readycnt++;
                                        mpu9250.readAccelData(accelCount);  // Read the x/y/z
                                        ax = (float)accelCount[0]*aRes - accelBias[0];  // Calculo da aceleração - mg
                                        ay = (float)accelCount[1]*aRes - accelBias[1];
                                        az = (float)accelCount[2]*aRes - accelBias[2];
                
                                        mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
                                        gx = (float)gyroCount[0]*gRes - gyroBias[0];  // calculo do giroscopio - rad/s
                                        gy = (float)gyroCount[1]*gRes - gyroBias[1];
                                        gz = (float)gyroCount[2]*gRes - gyroBias[2];
                            
                                        mpu9250.readMagData(magCount);  // Read the x/y/z adc values
                                        mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  //calculo do magnetometro
                                        my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
                                        mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
                                        }

                            Now = t.read_us();
                            deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
                            lastUpdate = Now;
                    
                            sum += deltat;
                            sumCount++;
                            
                            uint32_t us = t.read_us();
                            mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
                            us = t.read_us()-us;
                    
                            // Serial print and/or display at 0.5 s rate independent of data rates
                            delt_t = t.read_ms() - count;
                            
                            
        if (delt_t > 100) { //taxa de atualização calculos - ms
        
            readycnt=0;

            tempCount = mpu9250.readTempData();  // Read the adc values
            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade

            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            pitch *= 180.0f / PI;
            yaw   *= 180.0f / PI;
            yaw   -= 21.1f; // Declination em São carlos (27/06/19)
            roll  *= 180.0f / PI;

            count = t.read_ms();
            
            sum = 0;
            sumCount = 0;
              if(i==0){                                                          //Cálculo da norma inicial para ajuste
                 for(i=0;i<10;i++)
                 {
                     pr[i] = sqrt((1000000*ax*ax)+(1000000*ay*ay)+(1000000*az*az));
                     soma = soma + pr[i];
                 }
        
                       norma = soma/10000;
                     }
                                                                            //Cálculo das acelerações normalizadas
             axf = (ax*1000)/norma;
             ayf = (ay*1000)/norma;
             azf = (az*1000)/norma;
        
                                                                            //Print das acelerações    
            //  pc.printf("axf = %f\n", axf);
             // pc.printf("ayf = %f\n", ayf);
            //  pc.printf("azf = %f\n", azf);
                                                                             //Cálculo da norma para saber se existe aceleração
              norma2=sqrt((axf*axf)+(ayf*ayf)+(azf*azf));
              g=1000;         
    
                                                                            //Verifica se o botão está pressionado
              if (mybutton==0){ // Button is pressed
                 Flag_botao=1;
                 }
                 else{
                     Flag_botao=0;
                     }
                                                                                //Botão ativar DRS do visual studio
             if (0){ // comando abrir visual studio
                 Flag_vs=1;
                 }
                 else{
                     Flag_vs=0;
                     }                    
                                  
                                                                                //Verifica a norma2 para saber se existe aceleração    
            if(norma2>950){
                    Flag_Norma=1;  
                    LedNorma=1;       
                }
                else{
                    Flag_Norma=0;
                    LedNorma=0;
                }
                                           
                                                                            //Verifica a aceleração em x para saber se é constante ou positiva
            if(axf>-20){
                   Flag_axf=1;
                   LedAcelera=1;         
                }
                else{
                   LedAcelera=0;
                   Flag_axf=0; 
                }
                                                                            //Verifica se está desacelerando
            if(axf<-100){
                  Flag_naxf=1;         
                }
                else{
                  Flag_naxf=0; 
                }      
          
                                                                            //Cálculo do angulo do potenciometro\volante
            float angle = pot.read()*3600;
        
            if ((angle < 1050) && (angle > 950)){ 
                  LedVolante=1;
                  Flag_Volante= 1;
                }
                else{
                  LedVolante=0;
                  Flag_Volante=0; 
                }
                                                                            //Condições para acionar DRS  
            if((Flag_Volante==1&&Flag_axf==1&&Flag_Norma==1&&Flag_DRS==0)&&(Flag_botao==1||Flag_vs==1))  {
                  myled1 = 1;
                  myservo.period_ms(20);
                  myservo.pulsewidth_ms(2);
                //  pc.printf("drs ativadando\n");
                  Flag_DRS=1;
                  LedAsa=1;
                 } 
        
                                                                            //Mostra se depois de acionado o DRS continua ativado
             if(Flag_DRS==1){
               // pc.printf("drs ativo\n");
                }
                                                                            //Condições para desativar DRS
             if ((Flag_Volante==0 || Flag_axf==0)&&Flag_DRS==1) {
                 myled1 = 0;
                 myservo.period_ms(20);
                 myservo.pulsewidth_ms(1.5);
                 Flag_DRS=0;
                 LedAsa=0;
               }
                                                                            //Mostra se o DRS esta desativado
             if(Flag_DRS==0){
               
               }
          
          //comunicação visual studio
                
                if (inicio){
                    pc.puts("ready");
                    inicio = false;
                    }
              serial = pc.getc();
              
              if(serial == 'x'){
                  pc.printf("%f", (axf/norma2));
                  
                  
                }
              if(serial == 'y'){
                  pc.printf("%f", (ayf/norma2));
                  
                  
                }
                if(serial == 'd'){
                  if (Flag_DRS == 1) pc.printf("drson");
                  else pc.printf("drsoff");

                }
                
                if(serial == 'f'){
                    inicio = true;
                }
                
               
        }
       
    }

}