Diff: main.cpp

Revision:

3:d4736e223540

Parent:

1:fdfd9a35acc4

--- a/main.cpp	Mon May 16 13:19:53 2022 +0000
+++ b/main.cpp	Thu Aug 04 09:04:55 2022 +0000
@@ -112,7 +112,8 @@
 
 int main(void)
 {
-    pc.baud(230400);
+    //pc.baud(230400);
+    //pc.baud(460800);
     led2 = 1;
     led3 = 1;
     //IMU
@@ -163,10 +164,13 @@
     {
         
 //        printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],accel_[0],accel_[1],accel_[2],omega[0],omega[1],omega[2]);
+//        printf("%.4f,%.4f,%.4f,   %.4f,%.4f,%.4f,   %.4f,%.4f,%.4f,   %.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],   accel[0],accel[1],accel[2],  accel_[0],accel_[1],accel_[2],  accel__[0],accel__[1],accel__[2]);
 //        printf("%d,%d\n\r", GSS_X, GSS_Y);
 //        printf("%.3f,%.3f\r\n",velocityA, velocityB);  // velocityA or velocityB
 
         printf("%.3f,%.3f,%d,%d,%.4f,%.4f,%.4f\r\n",velocityA, velocityB, GSS_X, GSS_Y, accel_[0],accel_[1], omega[2]);
+
+//        printf("%.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
 //        printf("%.2f,%.2f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
     }
 }
@@ -272,7 +276,8 @@
             for(int j=0;j<3;j++){
                 uint32_t temp = (buffer[5+j*data_bytes]<<24) | (buffer[6+j*data_bytes]<<16) | (buffer[7+j*data_bytes]<<8) | (buffer[8+j*data_bytes]);
                 eul[j].data1 = temp;
-                euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
+                //euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
+                euler[j] = eul[j].data2;
             }
         }
         if(buffer[4+len1+1]== 0x40&&buffer[4+len1+2]== 0x20){
@@ -295,9 +300,20 @@
         }
 
     }
-    accel_[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
-    accel_[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
-    accel_[2] = accel[2];
+    eu[0] = euler[0]/180.0f*pi;
+    eu[1] = euler[1]/180.0f*pi;
+    eu[2] = 0; //Because euler[2] is not correct, set euler[2] = 0.
+    //deal with tilt angle ; *-1 because IMU on robot is 180 degree reverse
+    //Use Euler angles Z1X2Y3.
+    accel_[0] = (cos(eu[2])*cos(eu[1]))*accel[0] + (cos(eu[2])*sin(eu[1])*sin(eu[0])-cos(eu[0])*sin(eu[2]))*accel[1] + (sin(eu[2])*sin(eu[0])+cos(eu[2])*cos(eu[0])*sin(eu[1]))*accel[2];
+    accel_[1] = (cos(eu[1])*sin(eu[2]))*accel[0] + (cos(eu[2])*cos(eu[0])+sin(eu[2])*sin(eu[1])*sin(eu[0]))*accel[1] + (cos(eu[0])*sin(eu[2])*sin(eu[1])-cos(eu[2])*sin(eu[0]))*accel[2];
+    accel_[2] = -1.0f*(sin(eu[1]))*accel[0] + (cos(eu[1])*sin(eu[0]))*accel[1] + (cos(eu[1])*cos(eu[0]))*accel[2];
+    accel_[0] = accel_[0] * (-1.0f);
+    accel_[1] = accel_[1] * (-1.0f);
+    
+//    accel__[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
+//    accel__[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
+//    accel__[2] = accel[2];
 }
 
 void MTi2_Init(){
@@ -652,7 +668,7 @@
 
 void init_UART()
 {
-    pc.baud(230400);  // baud rate閮剔9600
+    pc.baud(460800);  // baud rate閮剔9600
     pc.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
 }