自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
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
diff -r a0b9b6e71e41 -r f2407caf81ba AUTOmatics.cpp --- /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
diff -r a0b9b6e71e41 -r f2407caf81ba AUTOmatics.hpp --- 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
diff -r a0b9b6e71e41 -r f2407caf81ba main.cpp --- 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)