足回り用プログラム(修正版)
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 0:a1238c4cd105
- Child:
- 1:ea880e226e5a
diff -r 000000000000 -r a1238c4cd105 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jan 19 12:43:26 2022 +0000 @@ -0,0 +1,76 @@ +#include "mbed.h" +#include "rotary_inc.hpp" +#include "PIDco.hpp" +#include "TARGETco.hpp" +#include "DUALSHOCKco.hpp" +#include "sbdbt.hpp" +#include "BNO055.h" + +#define SDA D3 +#define SCL D6 +#define PI 3.1415926535897 + +Serial pc(USBTX,USBRX); + +RotaryInc data_1(PA_14,PA_15,0); +RotaryInc data_2(PA_12,PC_5,0); +RotaryInc data_3(PC_0,PC_1,0); +RotaryInc data_4(PC_2,PC_3,0); + +PIDco pid_1; +PIDco pid_2; +PIDco pid_3; +PIDco pid_4; + +TARGETco TG; +BNO055 bno(SDA,SCL); +DUALSHOCKco DS; + +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); + bno.get_angles(); + theta = (360 - (double)bno.euler.yaw) / 180 * PI; + + if(theta > PI){ + theta = theta - 2 * PI; + } + + 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); + } + else{ + TG.pass_target(DS.obt_X()); + } + + pid_1.pass_val(data_1.get(),TG.obt_target1()); + pid_2.pass_val(data_2.get(),TG.obt_target2()); + pid_3.pass_val(data_3.get(),TG.obt_target3()); + pid_4.pass_val(data_4.get(),TG.obt_target4()); + + pid_1.wheel_ctl(PB_9,PB_8); + pid_2.wheel_ctl(PB_14,PB_13); + 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()); + while(Time.read_us() - timer <= 50 * 1000); + } +} + +//BNOのピン(PB_3,PB_10) + + + \ No newline at end of file