Takujyou_Ishii / Mbed 2 deprecated test_mpu6050

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Madgwickfilter MPU6050

main.cpp@6:94c4a0e7bf48, 2020-11-12 (annotated)

Committer:
aoikoizumi
Date:
Thu Nov 12 08:47:49 2020 +0000
Revision:
6:94c4a0e7bf48
Parent:
5:f41d7b3be417 
ugokimasen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stream3715 0:07431908151a 1 #include "mbed.h"
stream3715 0:07431908151a 2 #include "MPU6050.h"
shimizuta 5:f41d7b3be417 3 #include "madgwickfilter.h"
aoikoizumi 6:94c4a0e7bf48 4 MPU6050 mpu6050(D5,D7);
shimizuta 5:f41d7b3be417 5 Madgwickfilter filter;
shimizuta 5:f41d7b3be417 6 const double PI = 3.14159265358979323846f;
shimizuta 5:f41d7b3be417 7 const double kRad2Deg = 180.0/PI;
aoikoizumi 6:94c4a0e7bf48 8 Serial pc(USBTX,USBRX);
aoikoizumi 6:94c4a0e7bf48 9 PwmOut LED_f(LED1);
MazeTaka 4:fdba5e452d36 10
stream3715 0:07431908151a 11 int main()
stream3715 0:07431908151a 12 {
shimizuta 5:f41d7b3be417 13 mpu6050.init();
MazeTaka 4:fdba5e452d36 14 while(1) {
shimizuta 5:f41d7b3be417 15 //ジャイロから値取得
shimizuta 5:f41d7b3be417 16 mpu6050.CalMPU6050();
shimizuta 5:f41d7b3be417 17 double accel_x = mpu6050.GetAccelX();
shimizuta 5:f41d7b3be417 18 double accel_y = mpu6050.GetAccelY();
shimizuta 5:f41d7b3be417 19 double accel_z = mpu6050.GetAccelZ();
shimizuta 5:f41d7b3be417 20 double omega_x = mpu6050.GetXRadPerSec();
shimizuta 5:f41d7b3be417 21 double omega_y = mpu6050.GetYRadPerSec();
shimizuta 5:f41d7b3be417 22 double omega_z = mpu6050.GetZRadPerSec();
shimizuta 5:f41d7b3be417 23 //madgwickフィルターをかけ、yaw, pitch, rollを計算
shimizuta 5:f41d7b3be417 24 filter.Update(accel_x, accel_y, accel_z, omega_x, omega_y, omega_z);
aoikoizumi 6:94c4a0e7bf48 25 LED_f=filter.getYaw()*kRad2Deg/180;
aoikoizumi 6:94c4a0e7bf48 26 if(filter.getYaw()*kRad2Deg/180<=0.1){
aoikoizumi 6:94c4a0e7bf48 27 LED_f=0.1;
aoikoizumi 6:94c4a0e7bf48 28 }
aoikoizumi 6:94c4a0e7bf48 29 else if(filter.getYaw()*kRad2Deg/180>=0.9){
aoikoizumi 6:94c4a0e7bf48 30 LED_f=0.9;
aoikoizumi 6:94c4a0e7bf48 31 }
shimizuta 5:f41d7b3be417 32 printf("Yaw, Pitch, Roll[deg]: %9.4f %9.4f %9.4f\r\n",
shimizuta 5:f41d7b3be417 33 filter.getYaw()*kRad2Deg, filter.getPitch()*kRad2Deg, filter.getRoll()*kRad2Deg);
aoikoizumi 6:94c4a0e7bf48 34
stream3715 0:07431908151a 35 }
shimizuta 5:f41d7b3be417 36 }
aoikoizumi 6:94c4a0e7bf48 37
aoikoizumi 6:94c4a0e7bf48 38 /*
aoikoizumi 6:94c4a0e7bf48 39 #include "mbed.h"
aoikoizumi 6:94c4a0e7bf48 40 #include "madgwickfilter.h" //ライブラリをインクルード
aoikoizumi 6:94c4a0e7bf48 41
aoikoizumi 6:94c4a0e7bf48 42
aoikoizumi 6:94c4a0e7bf48 43 Madgwickfilter filter();
aoikoizumi 6:94c4a0e7bf48 44 Serial pc(USBTX, USBRX); // tx, rx
aoikoizumi 6:94c4a0e7bf48 45
aoikoizumi 6:94c4a0e7bf48 46 int main()
aoikoizumi 6:94c4a0e7bf48 47 {
aoikoizumi 6:94c4a0e7bf48 48 double x_acc,y_acc,z_acc;
aoikoizumi 6:94c4a0e7bf48 49 double x_rate,y_rate,z_rate;
aoikoizumi 6:94c4a0e7bf48 50 double roll,pitch,yaw;
aoikoizumi 6:94c4a0e7bf48 51 while(1){
aoikoizumi 6:94c4a0e7bf48 52 filter.Update(x_acc,y_acc,z_acc,x_rate,y_rate,z_rate);
aoikoizumi 6:94c4a0e7bf48 53 roll=filter.getRoll();
aoikoizumi 6:94c4a0e7bf48 54 pitch=filter.getPitch();
aoikoizumi 6:94c4a0e7bf48 55 yaw=filter.getYaw();
aoikoizumi 6:94c4a0e7bf48 56 pc.printf("roll:%8.4f,pitch:%8.4f,yaw:%8.4f\r\n",roll,pitch,yaw);
aoikoizumi 6:94c4a0e7bf48 57 }
aoikoizumi 6:94c4a0e7bf48 58 }
aoikoizumi 6:94c4a0e7bf48 59 */