Revision 8:f2407caf81ba, committed 2022-03-05

Comitter:

kazumayamanaka

Date:

Sat Mar 05 01:00:42 2022 +0000

Parent:

7:a0b9b6e71e41

Commit message:

program of motor

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AUTOmatics.cpp	Sat Mar 05 01:00:42 2022 +0000
@@ -0,0 +1,33 @@
+#include "AUTOmatics.hpp"
+
+void automatics::getTarget(double TARGETX,double TARGETY,double TARGETT){
+    targetX = TARGETX, targetY = TARGETY, targetT = TARGETT;
+}
+
+void automatics::getCurrent(double CURRENTX,double CURRENTY,double CURRENTT){
+    currentX = CURRENTX, currentY = CURRENTY, currentT = CURRENTT;
+}
+
+void automatics::Calc_Cp(){
+    xCp = MAXSPEED / targetX, yCp = MAXSPEED / targetY, tCp = MAXSPEED / targetT;
+}
+
+void automatics::Calc_diff(){
+    diffX = targetX - currentX, diffY = targetY - currentY , diffT = targetT - currentT;
+}
+
+void automatics::Calc_Pval(){
+    xP = diffX * xCp, yP = diffY * yCp, tP = diffT * tCp;
+}
+
+void automatics::Calc_PWM(){
+    xPWM += xP, yPWM += yP, tPWM += tP;
+}
+
+double automatics::get_xPWM(){ return xPWM; }
+
+double automatics::get_yPWM(){ return yPWM; }
+
+double automatics::get_tPWM(){ return tPWM; }
+
+    
\ No newline at end of file

--- a/AUTOmatics.hpp	Wed Feb 23 03:03:36 2022 +0000
+++ b/AUTOmatics.hpp	Sat Mar 05 01:00:42 2022 +0000
@@ -2,5 +2,27 @@
 #define AUTOMATICS_H
 #include "mbed.h"
 
+#define MAXSPEED 500
+
 class automatics{
-    automatics(
\ No newline at end of file
+    public:
+        void getTarget(double,double,double);
+        void getCurrent(double,double,double);
+        double get_xPWM(),get_yPWM(),get_tPWM();
+    private:
+        double targetX,targetY,targetT;
+        double currentX,currentY,currentT;
+        double diffX,diffY,diffT;
+        double xCp,yCp,tCp;
+        double xP,yP,tP;
+        double xPWM,yPWM,tPWM;
+        
+        void Calc_diff();
+        void Calc_Cp();
+        void Calc_PWM();
+        void Calc_Pval();
+};
+
+#endif
+        
+        
\ No newline at end of file

--- 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)