Revision 4:867c2b5cf81a, committed 2022-01-22

Comitter:

guesta

Date:

Sat Jan 22 16:00:04 2022 +0000

Parent:

3:6fb5c3218608

Child:

5:1a850f68a06c

Commit message:

add odometry_program;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/library/odometry.hpp	Sat Jan 22 16:00:04 2022 +0000
@@ -0,0 +1,37 @@
+#ifndef odometry_hpp
+#define odometry_hpp
+#include "mbed.h"
+#include "rotary_inc.hpp"
+
+double x_position;
+double y_position;
+double odometer_ensyu;
+double read_interval  = 0.05;
+
+RotaryInc odometer[4]  = {
+    RotaryInc(PC_2 ,PC_3 ,1,256,4),
+    RotaryInc(PA_12,PC_5 ,1,256,4),
+    RotaryInc(PC_0 ,PC_1 ,1,256,4),
+    RotaryInc(PC_4 ,PA_13,1,256,4)
+};
+
+void get_position(double pltheta){
+    double Vx[2] = {0,0};
+    double Vy[2] = {0,0};
+    double Vod[4];
+    
+    for(int i = 0;i < 4;i++){
+        Vod[i] = (odometer[i].getSpeed()) * odometer_ensyu / 10;
+    }
+    
+    Vx[1] = ((Vod[0] * cos(pltheta)) + (Vod[1] * cos(pltheta + PI / 2)) + (Vod[2] * cos(pltheta + PI)) + (Vod[3] * cos(pltheta + 3 * (PI / 2)))) / 2;
+    Vy[1] = ((Vod[3] * cos(pltheta)) + (Vod[0] * cos(pltheta + PI / 2)) + (Vod[1] * cos(pltheta + PI)) + (Vod[2] * cos(pltheta + 3 * (PI / 2)))) / 2;
+    
+    x_position += ((Vx[1] + Vx[0]) / 2) * read_interval;
+    y_position -= ((Vy[1] + Vy[0]) / 2) * read_interval;
+    
+    Vx[0] = Vx[1];
+    Vy[0] = Vy[1];
+}
+
+#endif
\ No newline at end of file

--- a/main.cpp	Fri Jan 21 09:08:26 2022 +0000
+++ b/main.cpp	Sat Jan 22 16:00:04 2022 +0000
@@ -7,6 +7,7 @@
 #include "BNO055.h"
 #include "scrp_slave.hpp"
 #include "function.hpp"
+#include "odometry.hpp"
 
 #define SDA D3
 #define SCL D6
@@ -40,6 +41,11 @@
 double theta;
 double X,Y;
 
+//for odometry
+double theta2;
+double pltheta;
+
+
 int main(){
     Time.start();
     TG.pass_val(0,0,0);
@@ -56,11 +62,19 @@
         bno.get_angles();
         
         theta = bno.euler.yaw * (PI / 180);
+        theta2 = (360 - bno.euler.yaw) * (PI /180);
         
         if(theta > PI){
             theta = -(2 * PI - theta);
         }
         
+        if(theta2 > PI){
+            pltheta = theta2 - 2 * PI;    
+        }else{
+            pltheta = theta2;    
+        }
+        
+        get_position(pltheta);
         DS.pass_val(x_component,y_component,r2_num,l2_num);
         
         if(DS.cal_input() == true){
@@ -101,7 +115,7 @@
             led4 = 0;
         }
         
-        printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
+        printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd(),x_position,y_position);
         while(Time.read_us() - timer <= 50 * 1000);
         }
 }