Diff: main.cpp

Revision:

2:c889fecf9afe

Parent:

1:b1562831bbaf

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