足回り用プログラム(修正版)
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 4:920acf362fb6
- Parent:
- 2:612efbe94f42
diff -r 5122dce42398 -r 920acf362fb6 main.cpp --- a/main.cpp Fri Jan 21 09:13:50 2022 +0000 +++ b/main.cpp Fri Jan 21 09:15:33 2022 +0000 @@ -5,50 +5,36 @@ #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 - -ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800); - + +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); - -DigitalOut led1(PA_10); -DigitalOut led2(PB_15); -DigitalOut led3(PB_2); -DigitalOut led4(PC_6); - + +sbdbt sb(PA_0,PA_1); + Timer Time; double timer; double theta; double X,Y; - + 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(); @@ -61,7 +47,7 @@ theta = -(2 * PI - theta); } - DS.pass_val(x_component,y_component,r2_num,l2_num); + 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); @@ -80,33 +66,9 @@ pid_3.wheel_ctl(PB_5,PB_4); pid_4.wheel_ctl(PB_7,PB_6); - 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()); + 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 + +//BNOのピン(PB_3,PB_10) \ No newline at end of file