自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
main.cpp@1:ea880e226e5a, 2022-01-20 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Thu Jan 20 07:27:53 2022 +0000
- Revision:
- 1:ea880e226e5a
- Parent:
- 0:a1238c4cd105
- Child:
- 2:e189d55ef292
- Child:
- 3:612efbe94f42
2022_ateam_motor_program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "mbed.h" |
kazumayamanaka | 0:a1238c4cd105 | 2 | #include "rotary_inc.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 3 | #include "PIDco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 4 | #include "TARGETco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 5 | #include "DUALSHOCKco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 6 | #include "sbdbt.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 7 | #include "BNO055.h" |
kazumayamanaka | 0:a1238c4cd105 | 8 | |
kazumayamanaka | 0:a1238c4cd105 | 9 | #define SDA D3 |
kazumayamanaka | 0:a1238c4cd105 | 10 | #define SCL D6 |
kazumayamanaka | 0:a1238c4cd105 | 11 | #define PI 3.1415926535897 |
kazumayamanaka | 0:a1238c4cd105 | 12 | |
kazumayamanaka | 0:a1238c4cd105 | 13 | Serial pc(USBTX,USBRX); |
kazumayamanaka | 0:a1238c4cd105 | 14 | |
kazumayamanaka | 0:a1238c4cd105 | 15 | RotaryInc data_1(PA_14,PA_15,0); |
kazumayamanaka | 0:a1238c4cd105 | 16 | RotaryInc data_2(PA_12,PC_5,0); |
kazumayamanaka | 0:a1238c4cd105 | 17 | RotaryInc data_3(PC_0,PC_1,0); |
kazumayamanaka | 0:a1238c4cd105 | 18 | RotaryInc data_4(PC_2,PC_3,0); |
kazumayamanaka | 0:a1238c4cd105 | 19 | |
kazumayamanaka | 0:a1238c4cd105 | 20 | PIDco pid_1; |
kazumayamanaka | 0:a1238c4cd105 | 21 | PIDco pid_2; |
kazumayamanaka | 0:a1238c4cd105 | 22 | PIDco pid_3; |
kazumayamanaka | 0:a1238c4cd105 | 23 | PIDco pid_4; |
kazumayamanaka | 0:a1238c4cd105 | 24 | |
kazumayamanaka | 0:a1238c4cd105 | 25 | TARGETco TG; |
kazumayamanaka | 0:a1238c4cd105 | 26 | BNO055 bno(SDA,SCL); |
kazumayamanaka | 0:a1238c4cd105 | 27 | DUALSHOCKco DS; |
kazumayamanaka | 0:a1238c4cd105 | 28 | |
kazumayamanaka | 0:a1238c4cd105 | 29 | sbdbt sb(PA_0,PA_1); |
kazumayamanaka | 0:a1238c4cd105 | 30 | |
kazumayamanaka | 0:a1238c4cd105 | 31 | Timer Time; |
kazumayamanaka | 0:a1238c4cd105 | 32 | double timer; |
kazumayamanaka | 0:a1238c4cd105 | 33 | double theta; |
kazumayamanaka | 0:a1238c4cd105 | 34 | double X,Y; |
kazumayamanaka | 0:a1238c4cd105 | 35 | |
kazumayamanaka | 0:a1238c4cd105 | 36 | int main(){ |
kazumayamanaka | 0:a1238c4cd105 | 37 | Time.start(); |
kazumayamanaka | 0:a1238c4cd105 | 38 | |
kazumayamanaka | 0:a1238c4cd105 | 39 | while(1){ |
kazumayamanaka | 0:a1238c4cd105 | 40 | timer = Time.read_us(); |
kazumayamanaka | 0:a1238c4cd105 | 41 | bno.setmode(OPERATION_MODE_IMUPLUS); |
kazumayamanaka | 0:a1238c4cd105 | 42 | bno.get_angles(); |
kazumayamanaka | 1:ea880e226e5a | 43 | |
kazumayamanaka | 1:ea880e226e5a | 44 | theta = bno.euler.yaw * (PI / 180); |
kazumayamanaka | 1:ea880e226e5a | 45 | |
kazumayamanaka | 0:a1238c4cd105 | 46 | if(theta > PI){ |
kazumayamanaka | 1:ea880e226e5a | 47 | theta = -(2 * PI - theta); |
kazumayamanaka | 0:a1238c4cd105 | 48 | } |
kazumayamanaka | 0:a1238c4cd105 | 49 | |
kazumayamanaka | 0:a1238c4cd105 | 50 | DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); |
kazumayamanaka | 0:a1238c4cd105 | 51 | |
kazumayamanaka | 0:a1238c4cd105 | 52 | if(DS.cal_input() == true){ |
kazumayamanaka | 0:a1238c4cd105 | 53 | TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); |
kazumayamanaka | 0:a1238c4cd105 | 54 | } |
kazumayamanaka | 0:a1238c4cd105 | 55 | else{ |
kazumayamanaka | 0:a1238c4cd105 | 56 | TG.pass_target(DS.obt_X()); |
kazumayamanaka | 0:a1238c4cd105 | 57 | } |
kazumayamanaka | 0:a1238c4cd105 | 58 | |
kazumayamanaka | 0:a1238c4cd105 | 59 | pid_1.pass_val(data_1.get(),TG.obt_target1()); |
kazumayamanaka | 0:a1238c4cd105 | 60 | pid_2.pass_val(data_2.get(),TG.obt_target2()); |
kazumayamanaka | 0:a1238c4cd105 | 61 | pid_3.pass_val(data_3.get(),TG.obt_target3()); |
kazumayamanaka | 0:a1238c4cd105 | 62 | pid_4.pass_val(data_4.get(),TG.obt_target4()); |
kazumayamanaka | 0:a1238c4cd105 | 63 | |
kazumayamanaka | 1:ea880e226e5a | 64 | pid_1.wheel_ctl(PC_9,PC_8); |
kazumayamanaka | 0:a1238c4cd105 | 65 | pid_2.wheel_ctl(PB_14,PB_13); |
kazumayamanaka | 0:a1238c4cd105 | 66 | pid_3.wheel_ctl(PB_5,PB_4); |
kazumayamanaka | 0:a1238c4cd105 | 67 | pid_4.wheel_ctl(PB_7,PB_6); |
kazumayamanaka | 0:a1238c4cd105 | 68 | |
kazumayamanaka | 0:a1238c4cd105 | 69 | 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()); |
kazumayamanaka | 0:a1238c4cd105 | 70 | while(Time.read_us() - timer <= 50 * 1000); |
kazumayamanaka | 0:a1238c4cd105 | 71 | } |
kazumayamanaka | 0:a1238c4cd105 | 72 | } |
kazumayamanaka | 0:a1238c4cd105 | 73 | |
kazumayamanaka | 0:a1238c4cd105 | 74 | //BNOのピン(PB_3,PB_10) |
kazumayamanaka | 0:a1238c4cd105 | 75 | |
kazumayamanaka | 0:a1238c4cd105 | 76 | |
kazumayamanaka | 0:a1238c4cd105 | 77 |