2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

Diff: main.cpp

Revision:
4:867c2b5cf81a
Parent:
2:612efbe94f42
Child:
5:1a850f68a06c
--- 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);
         }
 }