Diff: main.cpp

Revision:

7:a0b9b6e71e41

Parent:

2:e189d55ef292

Child:

8:f2407caf81ba

--- a/main.cpp	Fri Jan 28 15:13:35 2022 +0000
+++ b/main.cpp	Wed Feb 23 03:03:36 2022 +0000
@@ -6,7 +6,6 @@
 #include "Odmetry.hpp"
 #include "sbdbt.hpp"
 #include "BNO055.h"
-
 #define SDA D3
 #define SCL D6
 #define PI 3.1415926535897
@@ -35,18 +34,30 @@
 sbdbt sb(PA_0,PA_1);
 
 Timer Time;
+bool mode=false;
 double timer;
 double theta;
+double automaticsX(double PosX);
+double automaticsY(double PosY);
+double automaticsTHETA(double THETA);
+void automatics();
 
 int main(){
     Time.start();
-    
+    bno.reset();
     
     while(1){
         timer = Time.read_us();
         bno.setmode(OPERATION_MODE_IMUPLUS);
         bno.get_angles();
         
+        if(sb.circle()!=0){
+            mode = true;
+        }
+        if(sb.square()!=0){
+            mode = false;
+        }
+        
         theta = bno.euler.yaw * (PI / 180);
         
         if(theta > PI){
@@ -55,13 +66,18 @@
         
         theta = theta - PI / 4;
         
-        DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
+        if(mode == false){
+            DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
         
-        if(DS.cal_input() == true){
-            TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
+            if(DS.cal_input() == true){
+                TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
+            }
+            else{
+                TG.pass_target(DS.obt_X());
+            }
         }
         else{
-            TG.pass_target(DS.obt_X());
+            automatics();
         }
         
         pid_1.pass_val(data_1.get(),TG.obt_target1());
@@ -80,8 +96,49 @@
         //pc.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());
         while(Time.read_us() - timer <= 50 * 1000);
         }
+        automatics();
 }
 
+double automaticsX(double PosX){
+    if(PosX > 0){
+        if(PosX <= odmetry.obt_CurrentPosX())return 0;
+        else return 100;
+    }
+    else{
+        if(PosX >= odmetry.obt_CurrentPosX())return 0;
+        else return -100;
+    }
+}
+double automaticsY(double PosY){
+    if(PosY > 0){
+        if(PosY <= odmetry.obt_CurrentPosY())return 0;
+        else return -100;
+    }
+    else{
+        if(PosY >= odmetry.obt_CurrentPosY())return 0;
+        else return 100;
+    }
+}
+double automaticsTHETA(double THETA){
+    if(THETA > 0){
+        if(THETA <= odmetry.obt_CurrentTheta())return 0;
+        else return -70;
+    }
+    else{
+        if(THETA >= odmetry.obt_CurrentTheta())return 0;
+        else return 70;
+    }
+}
+void automatics(){
+    if(automaticsY(100) != 0 || automaticsX(500) !=0){ 
+        TG.pass_val(automaticsY(100),automaticsX(500),theta);
+    }
+    else{
+        TG.pass_target(automaticsTHETA(PI / 4));
+    }
+}
+    
+
 //BNOのピン(PB_3,PB_10)