自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 2:e189d55ef292
- Parent:
- 1:ea880e226e5a
- Child:
- 7:a0b9b6e71e41
diff -r ea880e226e5a -r e189d55ef292 main.cpp --- a/main.cpp Thu Jan 20 07:27:53 2022 +0000 +++ b/main.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -3,6 +3,7 @@ #include "PIDco.hpp" #include "TARGETco.hpp" #include "DUALSHOCKco.hpp" +#include "Odmetry.hpp" #include "sbdbt.hpp" #include "BNO055.h" @@ -17,6 +18,11 @@ RotaryInc data_3(PC_0,PC_1,0); RotaryInc data_4(PC_2,PC_3,0); +RotaryInc MW_1(PA_13,PC_4,0); +RotaryInc MW_2(PC_10,PC_11,0); +RotaryInc MW_3(PA_7,PA_6,0); +RotaryInc MW_4(PA_8,PA_9,0); + PIDco pid_1; PIDco pid_2; PIDco pid_3; @@ -25,17 +31,17 @@ TARGETco TG; BNO055 bno(SDA,SCL); DUALSHOCKco DS; - +Odmetry odmetry; sbdbt sb(PA_0,PA_1); Timer Time; double timer; double theta; -double X,Y; int main(){ Time.start(); + while(1){ timer = Time.read_us(); bno.setmode(OPERATION_MODE_IMUPLUS); @@ -47,6 +53,8 @@ theta = -(2 * PI - theta); } + theta = theta - PI / 4; + DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); if(DS.cal_input() == true){ @@ -66,7 +74,10 @@ pid_3.wheel_ctl(PB_5,PB_4); pid_4.wheel_ctl(PB_7,PB_6); - 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.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()); while(Time.read_us() - timer <= 50 * 1000); } }