Diff: main.cpp

Revision:

8:f2407caf81ba

Parent:

7:a0b9b6e71e41

--- a/main.cpp	Wed Feb 23 03:03:36 2022 +0000
+++ b/main.cpp	Sat Mar 05 01:00:42 2022 +0000
@@ -6,9 +6,12 @@
 #include "Odmetry.hpp"
 #include "sbdbt.hpp"
 #include "BNO055.h"
+#include "AUTOmatics.hpp"
 #define SDA D3
 #define SCL D6
 #define PI 3.1415926535897
+#define NORMALMODE true
+#define ROUNDMODE false
 
 Serial pc(USBTX,USBRX);
 
@@ -41,6 +44,11 @@
 double automaticsY(double PosY);
 double automaticsTHETA(double THETA);
 void automatics();
+void Testautomatics();
+void diff(double PosX,double PosY,double PosT);
+double diffX = 0,diffY = 0,diffT = 0;
+double targetX=500,targetY=100,targetT=PI / 4;
+bool set = true;
 
 int main(){
     Time.start();
@@ -78,6 +86,7 @@
         }
         else{
             automatics();
+            pc.printf("%d  ",set);
         }
         
         pid_1.pass_val(data_1.get(),TG.obt_target1());
@@ -92,51 +101,53 @@
         
         odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
         odmetry.pass_angle(theta);
-        odmetry.print_Pos();
-        //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());
+        //odmetry.print_Pos();
+        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;
+
+double automaticsX(){
+    if(targetX > 0){
+        return diffX * 500 / targetX;
     }
     else{
-        if(PosX >= odmetry.obt_CurrentPosX())return 0;
-        else return -100;
+        return -diffX * 500 / targetX;
     }
 }
-double automaticsY(double PosY){
-    if(PosY > 0){
-        if(PosY <= odmetry.obt_CurrentPosY())return 0;
-        else return -100;
+double automaticsY(){
+    if(targetY > 0){
+        return -diffY * 500 / targetY;
     }
     else{
-        if(PosY >= odmetry.obt_CurrentPosY())return 0;
-        else return 100;
+        return diffY * 500 / targetY;
     }
 }
-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;
-    }
+double automaticsTHETA(){
+    if(targetT > 0)return -diffT * 500 / targetT;
+    else return diffT * 500 / targetT;
 }
 void automatics(){
-    if(automaticsY(100) != 0 || automaticsX(500) !=0){ 
-        TG.pass_val(automaticsY(100),automaticsX(500),theta);
+    diff(targetX,targetY,targetT);
+    
+    if((diffX > 1)||(diffY > 1)){
+        TG.pass_val(automaticsY(),automaticsX(),theta);
+        set = true;
     }
-    else{
-        TG.pass_target(automaticsTHETA(PI / 4));
-    }
+    else set = false;
+    
+    if(set == false)TG.pass_target(automaticsTHETA());
+    
+    printf("  %lf %lf  ",diffX,diffY);
 }
+
+void diff(double PosX,double PosY,double PosT){
+    diffX = PosX - odmetry.obt_CurrentPosX();
+    diffY = PosY - odmetry.obt_CurrentPosY();
+    diffT = PosT - odmetry.obt_CurrentTheta();
+}
+    
     
 
 //BNOのピン(PB_3,PB_10)