足回り用プログラム(修正版)
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 1:ea880e226e5a
- Parent:
- 0:a1238c4cd105
- Child:
- 2:612efbe94f42
- Child:
- 5:e189d55ef292
--- a/main.cpp Wed Jan 19 12:43:26 2022 +0000 +++ b/main.cpp Thu Jan 20 07:27:53 2022 +0000 @@ -40,10 +40,11 @@ timer = Time.read_us(); bno.setmode(OPERATION_MODE_IMUPLUS); bno.get_angles(); - theta = (360 - (double)bno.euler.yaw) / 180 * PI; - + + theta = bno.euler.yaw * (PI / 180); + if(theta > PI){ - theta = theta - 2 * PI; + theta = -(2 * PI - theta); } DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); @@ -60,7 +61,7 @@ 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_1.wheel_ctl(PC_9,PC_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);