2020_TeamA / Mbed 2 deprecated Yoshida_MiniRobo

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 YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C

Diff: main.cpp

Revision:
21:22b863d32705
Parent:
20:79f96d223e5d
Child:
22:9d77148a3f09
diff -r 79f96d223e5d -r 22b863d32705 main.cpp
--- a/main.cpp	Thu Mar 19 07:50:17 2020 +0000
+++ b/main.cpp	Fri Mar 20 04:12:22 2020 +0000
@@ -232,10 +232,12 @@
 *******************************************************************************/
 void LocEstimate(void)
 {
-    static double GyroDeg[2]= {0};
+     static double GyroDeg[2]= {0};
     static double EncDeg[2][2]= {0};
     static double disp[3]= {0};
 
+ 
+
     /* ジャイロの値取得 */
     bno.get_angles();
     GyroDeg[1]=GyroDeg[0];
@@ -248,18 +250,19 @@
         /* 差を求める*/
         disp[2]=GyroDeg[1]-GyroDeg[0];
     }
-    /* Enc1つの差分求める */
-    for(int i=0; i<1; i++) {
+    /* Enc2つの差分求める */
+    for(int i=0; i<2; i++) {
         EncDeg[i][1]=EncDeg[i][0];
         EncDeg[i][0]=EncoderDeg[i];
-        disp[i]=EncDeg[i][1]-EncDeg[i][0];
+        disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
     }
     /* 差分を加速度として保存 */
     NowAcc.theta = disp[2];
-    NowAcc.X =  -disp[0] * cos(NowLoc.theta);
-
+    NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
+    NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
     /* 差分を累積して現在位置を保存 */
     NowLoc.X += NowAcc.X;
+    NowLoc.Y += NowAcc.Y;
     NowLoc.theta += NowAcc.theta;
 }