曾 宗圓 / Mbed 2 deprecated LSM9DS1_project

Implement sensor fusion algorithm based on LSM9DS1 IMU.

Dependencies:   mbed

Fork of LSM9DS1_project by 曾 宗圓

Files at this revision

API Documentation at this revision

Revision 2:c889fecf9afe, committed 2018-04-24

Comitter:
middleyuan
Date:
Tue Apr 24 15:13:58 2018 +0000
Parent:
1:b1562831bbaf
Commit message:
SensorFusion

Changed in this revision

AS5145.cpp Show annotated file Show diff for this revision Revisions of this file
AS5145.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/AS5145.cpp	Wed Dec 20 15:45:02 2017 +0000
+++ b/AS5145.cpp	Tue Apr 24 15:13:58 2018 +0000
@@ -3,7 +3,7 @@
 SPI spi(PB_15,PB_14,PB_13);
 SPI spi2(PC_12,PC_11,PC_10);
 DigitalOut CS(PB_1);
-DigitalOut CS2(PA_14);
+DigitalOut CS2(PA_13);
 
 int position[2] = {0,0};
 int position_old[2] = {0,0};

--- a/AS5145.h	Wed Dec 20 15:45:02 2017 +0000
+++ b/AS5145.h	Tue Apr 24 15:13:58 2018 +0000
@@ -1,4 +1,4 @@
-#ifndef AS5145_H_
+#ifndef AS5145_H
 #define AS5145_H
 
 #include "mbed.h"

--- a/main.cpp	Wed Dec 20 15:45:02 2017 +0000
+++ b/main.cpp	Tue Apr 24 15:13:58 2018 +0000
@@ -2,13 +2,13 @@
 #include "LSM9DS1.h"
 #include "AS5145.h"
 
-LSM9DS1 imu3(D5, D7);
-LSM9DS1 imu2(D3, D6);
+LSM9DS1 imu2(D5, D7);
+LSM9DS1 imu3(D3, D6);
 LSM9DS1 imu(D14, D15);
-AnalogIn sEMG(PB_0);
-AnalogIn sEMG2(PC_0);
-AnalogIn sEMG3(PC_2);
-AnalogIn sEMG4(PC_3);
+AnalogIn sEMG(D13);
+AnalogIn sEMG2(D1);
+AnalogIn sEMG3(D0);
+AnalogIn sEMG4(PC_4);
 Serial pc(USBTX, USBRX);
 Ticker timer1;
 Ticker timer2;
@@ -83,10 +83,10 @@
     init_TIMER();
     while (1)
     {
-        pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[0]*360/4096, position[1]*360/4096);
-        wait(0.1);
+        //pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096);
+        wait(0.05);
         //pc.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]);
-        //pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[0]*360/4096, position[1]*360/4096);
+        pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096);
         //pc.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]);
         //pc.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]);
         //pc.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]);
@@ -170,7 +170,7 @@
     w3[2]  = Gyro_axis_data_f[8];
     
     
-    estimator(axm,aym,azm,w3,w2,w1,20);
+    estimator(axm,aym,azm,w3,w2,w1,120);
     angle_fn(x1_hat,x2_hat);
     pitch_dot_fn(w3,w2,w1,sinroll,cosroll);
     pitch_double_dot_fn(pitch_dot,pitch_dot_old);