自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 3:612efbe94f42
- Parent:
- 1:ea880e226e5a
- Child:
- 5:920acf362fb6
diff -r ea880e226e5a -r 612efbe94f42 main.cpp --- a/main.cpp Thu Jan 20 07:27:53 2022 +0000 +++ b/main.cpp Fri Jan 21 08:57:07 2022 +0000 @@ -5,12 +5,14 @@ #include "DUALSHOCKco.hpp" #include "sbdbt.hpp" #include "BNO055.h" +#include "scrp_slave.hpp" +#include "function.hpp" #define SDA D3 #define SCL D6 #define PI 3.1415926535897 -Serial pc(USBTX,USBRX); +ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800); RotaryInc data_1(PA_14,PA_15,0); RotaryInc data_2(PA_12,PC_5,0); @@ -26,7 +28,12 @@ BNO055 bno(SDA,SCL); DUALSHOCKco DS; -sbdbt sb(PA_0,PA_1); +//sbdbt sb(PA_0,PA_1); + +DigitalOut led1(PA_10); +DigitalOut led2(PB_15); +DigitalOut led3(PB_2); +DigitalOut led4(PC_6); Timer Time; double timer; @@ -35,6 +42,13 @@ int main(){ Time.start(); + TG.pass_val(0,0,0); + scrp.addCMD(1,getLstick_x); + scrp.addCMD(2,getLstick_y); + scrp.addCMD(3,getL2); + scrp.addCMD(4,getR2); + scrp.addCMD(5,change_mode); + while(1){ timer = Time.read_us(); @@ -47,7 +61,7 @@ theta = -(2 * PI - theta); } - DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); + DS.pass_val(x_component,y_component,r2_num,l2_num); if(DS.cal_input() == true){ TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); @@ -66,7 +80,28 @@ 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()); + if(TG.obt_target1() != 0){ + led1 = 1; + }else{ + led1 = 0; + } + if(TG.obt_target2() != 0){ + led2 = 1; + }else{ + led2 = 0; + } + if(TG.obt_target3() != 0){ + led3 = 1; + }else{ + led3 = 0; + } + if(TG.obt_target4() != 0){ + led4 = 1; + }else{ + led4 = 0; + } + + 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); } }