Acelerometro formula
Dependencies: mbed Servo DebounceIn
Diff: main.cpp
- Revision:
- 3:6e4ca952e920
- Parent:
- 2:7b7060835269
--- a/main.cpp Wed Jun 26 00:02:36 2019 +0000
+++ b/main.cpp Tue Aug 27 22:51:39 2019 +0000
@@ -1,177 +1,96 @@
-/* MPU9250 Basic Example Code
- by: Kris Winer
- date: April 1, 2014
- license: Beerware - Use this code however you'd like. If you
- find it useful you can buy me a beer some time.
-
- Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
- getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
- allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
- Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
-
- SDA and SCL should have external pull-up resistors (to 3.3V).
- 10k resistors are on the EMSENSR-9250 breakout board.
-
- Hardware setup:
- MPU9250 Breakout --------- Arduino
- VDD ---------------------- 3.3V
- VDDI --------------------- 3.3V
- SDA ----------------------- A4
- SCL ----------------------- A5
- GND ---------------------- GND
-
- Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
- Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
- We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
- We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
- */
-
-//#include "ST_F401_84MHZ.h"
-//F401_init84 myinit(0);
#include "mbed.h"
#include "MPU9250.h"
-#include "math.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;
-MPU9250 mpu9250;
-
-int i;
+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;
-DigitalOut myled(LED1);
-
-float axf=0, ayf=0, azf=0, soma=0, naxf=0, Flag_botao=0, Flag_naxf=0, norma=0, norma2=0, g=0, Flag_Norma=0, Flag_axf=0, Flag_Volante=1;
-float pr[10];
+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
-InterruptIn button1(USER_BUTTON);
-volatile bool button1_pressed = false; // Used in the main loop
-volatile bool button1_enabled = true; // Used for debouncing
-Timeout button1_timeout; // Used for debouncing
-
-// Enables button when bouncing is over
-void button1_enabled_cb(void)
-{
- button1_enabled = true;
-}
+void mpuisr()
+ {
+ newData=true;
+ }
-// ISR handling button pressed event
-void button1_onpressed_cb(void)
-{
- if (button1_enabled) { // Disabled while the button is bouncing
- button1_enabled = false;
- button1_pressed = true; // To be read by the main loop
- button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
- }
-}
-
-void mpuisr()
-{
- newData=true;
-}
-
-int main()
-{
+int main(){
+
pc.baud(9600);
//Set up I2C
- i2c.frequency(400000); // use fast (400 kHz) I2C
-
- //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
-
+ i2c.frequency(400000);
t.start();
isrPin.rise(&mpuisr);
-
- // Read the WHO_AM_I register, this is a good test of communication
- uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
- //pc.printf("I AM 0x%x\n\r", whoami);
- //pc.printf("I SHOULD BE 0x71\n\r");
-
- if (whoami == 0x73) { // WHO_AM_I should always be 0x68
- //pc.printf("MPU9250 is online...\n\r");
- wait(1);
-
+
+ myservo.period_ms(20);
+ myservo.pulsewidth_ms(1.5);
+
+ uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // numero registro I2C MPU9250
- mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
- mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
- //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
- //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
- //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
- //pc.printf("x accel bias = %f\n\r", accelBias[0]);
- //pc.printf("y accel bias = %f\n\r", accelBias[1]);
- //pc.printf("z accel bias = %f\n\r", accelBias[2]);
- wait(2);
- mpu9250.initMPU9250();
- //pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
- mpu9250.initAK8963(magCalibration);
- //pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
- //pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
- //pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
- //if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
- //if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
- //if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
- //if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
- wait(2);
- }
- else {
- //pc.printf("Could not connect to MPU9250: \n\r");
- //pc.printf("%#x \n", whoami);
+ 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
+ }
-
- while(1) ; // Loop forever if communication doesn't happen
- }
+ 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
- mpu9250.getAres(); // Get accelerometer sensitivity
- mpu9250.getGres(); // Get gyro sensitivity
- mpu9250.getMres(); // Get magnetometer sensitivity
- //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
- //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
- //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
- 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) { //abertur do primeiro while
+ while(1) {
static int readycnt=0;
- // If intPin goes high, all data registers have new data
-
#if USE_ISR
- if(newData) {//if num 1
+ if(newData) {
newData=false;
mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS); //? need this with ISR
#else
- if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { //if num2 // On interrupt, check if data ready interrupt
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
#endif
readycnt++;
- mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
- // Now we'll calculate the accleration value into actual g's
- ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ 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
- // Calculate the gyro value into actual degrees per second
- gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ 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
- // Calculate the magnetometer values in milliGauss
- // Include factory calibration per data sheet and user environmental corrections
- mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
+ 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];
- }//if num 2
+ }
Now = t.read_us();
deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
@@ -179,136 +98,165 @@
sum += deltat;
sumCount++;
-
- // Pass gyro rate as rad/s
+
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;
- // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = t.read_ms() - count;
- if (delt_t > 500) { // update LCD once per half-second independent of read rate //if num 3
- //pc.printf("readycnt %d us %d\n",readycnt,us);
+
+
+ if (delt_t > 100) { //taxa de atualização calculos - ms
+
readycnt=0;
- //pc.printf("ax = %f", 1000*ax);
- //pc.printf(" ay = %f", 1000*ay);
- //pc.printf(" az = %f mg\n\r", 1000*az);
-
- //pc.printf("gx = %f", gx);
- //pc.printf(" gy = %f", gy);
- //pc.printf(" gz = %f deg/s\n\r", gz);
-
- //pc.printf("gx = %f", mx);
- //pc.printf(" gy = %f", my);
- //pc.printf(" gz = %f mG\n\r", mz);
tempCount = mpu9250.readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
- //pc.printf("temperature = %f C\n\r", temperature);
-
- //pc.printf("q0 = %f\n\r", q[0]);
- //pc.printf("q1 = %f\n\r", q[1]);
- //pc.printf("q2 = %f\n\r", q[2]);
- //pc.printf("q3 = %f\n\r", q[3]);
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 -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+ yaw -= 21.1f; // Declination em São carlos (27/06/19)
roll *= 180.0f / PI;
- //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
- //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-
- //myled= !myled;
count = t.read_ms();
+
sum = 0;
sumCount = 0;
- if(i==0){
- for(i=0;i<10;i++)
- {
- pr[i] = sqrt((1000000*ax*ax)+(1000000*ay*ay)+(1000000*az*az));
- soma = soma + pr[i];
- }
+ 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);
- }
- axf = (ax*1000)/norma;
- ayf = (ay*1000)/norma;
- azf = (az*1000)/norma;
-
- //pc.printf("norma = %f\n", norma);
- pc.printf("axf = %f\n", axf);
- pc.printf("ayf = %f\n", ayf);
- pc.printf("azf = %f\n", azf);
-
- norma2=sqrt((axf*axf)+(ayf*ayf)+(azf*azf));
- g=1000;
+ norma = soma/10000;
+ }
+ //Cálculo das acelerações normalizadas
+ axf = (ax*1000)/norma;
+ ayf = (ay*1000)/norma;
+ azf = (az*1000)/norma;
- button1.fall(callback(button1_onpressed_cb));
-
- if (button1_pressed) {
- button1_pressed = false;
- Flag_botao=1;
- };
-
- if(norma2>1000){
- Flag_Norma=1;
- }else{
- Flag_Norma=0;
+ //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;
}
-
- if(axf>-10){
- Flag_axf=1;
- }else{
- Flag_axf=0;
+ 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;
}
-
- if(naxf<-100){
- Flag_naxf=1;
- }else{
- Flag_naxf=0;
+ else{
+ LedAcelera=0;
+ Flag_axf=0;
+ }
+ //Verifica se está desacelerando
+ if(axf<-100){
+ Flag_naxf=1;
+ }
+ else{
+ Flag_naxf=0;
}
-
- float angle = pot.read()*3600;
+ //Cálculo do angulo do potenciometro\volante
+ float angle = pot.read()*3600;
- if ((angle < 1050) && (angle > 950) )
- {
- Flag_Volante= 1;
- }else{
- Flag_Volante=0;
+ 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");
}
-
- if(Flag_Volante==1&&Flag_axf==1&&Flag_botao==1&&Flag_DRS=0) {
- myled = 0;
- Flag_botao=0;
- pc.printf("drs disponivel\n");
- Flag_DRS=1;
- } else{
- pc.printf("drs nao disponivel\n");
- }
-
- if (Flag_Volante==0 || Flag_naxf==1) {
- myled = 1;
- pc.printf("desativando drs\n");
- Flag_DRS=0;
- };
-
-
- pc.printf("angulo: %f\n", angle);
- pc.printf("percentage: %3.3f%%\n", pot.read()*100.0f);
- pc.printf("Flag Volante %f\n", Flag_Volante);
- pc.printf("Flag axf %f\n", Flag_axf);
- pc.printf("Flag Norma %f\n", Flag_Norma);
- pc.printf("Norma %f\n\n", norma2);
- wait(0.2);
-
-
+ //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;
+ }
+
+
}
}