2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
main.cpp@5:1a850f68a06c, 2022-01-23 (annotated)
- Committer:
- guesta
- Date:
- Sun Jan 23 13:31:36 2022 +0000
- Revision:
- 5:1a850f68a06c
- Parent:
- 4:867c2b5cf81a
- Child:
- 6:e089fda81b74
add go to target_position;
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" |
guesta | 2:612efbe94f42 | 8 | #include "scrp_slave.hpp" |
guesta | 2:612efbe94f42 | 9 | #include "function.hpp" |
guesta | 4:867c2b5cf81a | 10 | #include "odometry.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 11 | |
kazumayamanaka | 0:a1238c4cd105 | 12 | #define SDA D3 |
kazumayamanaka | 0:a1238c4cd105 | 13 | #define SCL D6 |
kazumayamanaka | 0:a1238c4cd105 | 14 | #define PI 3.1415926535897 |
kazumayamanaka | 0:a1238c4cd105 | 15 | |
guesta | 2:612efbe94f42 | 16 | ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800); |
kazumayamanaka | 0:a1238c4cd105 | 17 | |
kazumayamanaka | 0:a1238c4cd105 | 18 | RotaryInc data_1(PA_14,PA_15,0); |
kazumayamanaka | 0:a1238c4cd105 | 19 | RotaryInc data_2(PA_12,PC_5,0); |
kazumayamanaka | 0:a1238c4cd105 | 20 | RotaryInc data_3(PC_0,PC_1,0); |
kazumayamanaka | 0:a1238c4cd105 | 21 | RotaryInc data_4(PC_2,PC_3,0); |
kazumayamanaka | 0:a1238c4cd105 | 22 | |
kazumayamanaka | 0:a1238c4cd105 | 23 | PIDco pid_1; |
kazumayamanaka | 0:a1238c4cd105 | 24 | PIDco pid_2; |
kazumayamanaka | 0:a1238c4cd105 | 25 | PIDco pid_3; |
kazumayamanaka | 0:a1238c4cd105 | 26 | PIDco pid_4; |
kazumayamanaka | 0:a1238c4cd105 | 27 | |
kazumayamanaka | 0:a1238c4cd105 | 28 | TARGETco TG; |
kazumayamanaka | 0:a1238c4cd105 | 29 | BNO055 bno(SDA,SCL); |
kazumayamanaka | 0:a1238c4cd105 | 30 | DUALSHOCKco DS; |
kazumayamanaka | 0:a1238c4cd105 | 31 | |
guesta | 2:612efbe94f42 | 32 | //sbdbt sb(PA_0,PA_1); |
guesta | 2:612efbe94f42 | 33 | |
guesta | 2:612efbe94f42 | 34 | DigitalOut led1(PA_10); |
guesta | 2:612efbe94f42 | 35 | DigitalOut led2(PB_15); |
guesta | 2:612efbe94f42 | 36 | DigitalOut led3(PB_2); |
guesta | 2:612efbe94f42 | 37 | DigitalOut led4(PC_6); |
kazumayamanaka | 0:a1238c4cd105 | 38 | |
kazumayamanaka | 0:a1238c4cd105 | 39 | Timer Time; |
kazumayamanaka | 0:a1238c4cd105 | 40 | double timer; |
kazumayamanaka | 0:a1238c4cd105 | 41 | double theta; |
kazumayamanaka | 0:a1238c4cd105 | 42 | double X,Y; |
kazumayamanaka | 0:a1238c4cd105 | 43 | |
guesta | 4:867c2b5cf81a | 44 | //for odometry |
guesta | 4:867c2b5cf81a | 45 | double theta2; |
guesta | 4:867c2b5cf81a | 46 | double pltheta; |
guesta | 4:867c2b5cf81a | 47 | |
guesta | 5:1a850f68a06c | 48 | double x_pos_target[] = {2}; |
guesta | 5:1a850f68a06c | 49 | double y_pos_target[] = {2}; |
guesta | 4:867c2b5cf81a | 50 | |
kazumayamanaka | 0:a1238c4cd105 | 51 | int main(){ |
kazumayamanaka | 0:a1238c4cd105 | 52 | Time.start(); |
guesta | 2:612efbe94f42 | 53 | TG.pass_val(0,0,0); |
guesta | 2:612efbe94f42 | 54 | scrp.addCMD(1,getLstick_x); |
guesta | 2:612efbe94f42 | 55 | scrp.addCMD(2,getLstick_y); |
guesta | 2:612efbe94f42 | 56 | scrp.addCMD(3,getL2); |
guesta | 2:612efbe94f42 | 57 | scrp.addCMD(4,getR2); |
guesta | 2:612efbe94f42 | 58 | scrp.addCMD(5,change_mode); |
guesta | 2:612efbe94f42 | 59 | |
kazumayamanaka | 0:a1238c4cd105 | 60 | |
kazumayamanaka | 0:a1238c4cd105 | 61 | while(1){ |
kazumayamanaka | 0:a1238c4cd105 | 62 | timer = Time.read_us(); |
kazumayamanaka | 0:a1238c4cd105 | 63 | bno.setmode(OPERATION_MODE_IMUPLUS); |
kazumayamanaka | 0:a1238c4cd105 | 64 | bno.get_angles(); |
kazumayamanaka | 1:ea880e226e5a | 65 | |
kazumayamanaka | 1:ea880e226e5a | 66 | theta = bno.euler.yaw * (PI / 180); |
guesta | 4:867c2b5cf81a | 67 | theta2 = (360 - bno.euler.yaw) * (PI /180); |
kazumayamanaka | 1:ea880e226e5a | 68 | |
kazumayamanaka | 0:a1238c4cd105 | 69 | if(theta > PI){ |
kazumayamanaka | 1:ea880e226e5a | 70 | theta = -(2 * PI - theta); |
kazumayamanaka | 0:a1238c4cd105 | 71 | } |
kazumayamanaka | 0:a1238c4cd105 | 72 | |
guesta | 4:867c2b5cf81a | 73 | if(theta2 > PI){ |
guesta | 5:1a850f68a06c | 74 | pltheta = theta2 - 2 * PI; |
guesta | 4:867c2b5cf81a | 75 | }else{ |
guesta | 4:867c2b5cf81a | 76 | pltheta = theta2; |
guesta | 4:867c2b5cf81a | 77 | } |
guesta | 4:867c2b5cf81a | 78 | |
guesta | 4:867c2b5cf81a | 79 | get_position(pltheta); |
guesta | 2:612efbe94f42 | 80 | DS.pass_val(x_component,y_component,r2_num,l2_num); |
kazumayamanaka | 0:a1238c4cd105 | 81 | |
guesta | 5:1a850f68a06c | 82 | if(auto_mode == false){ |
guesta | 5:1a850f68a06c | 83 | if(DS.cal_input() == true){ |
guesta | 5:1a850f68a06c | 84 | TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); |
guesta | 5:1a850f68a06c | 85 | }else{ |
guesta | 5:1a850f68a06c | 86 | TG.pass_target(DS.obt_X()); |
guesta | 5:1a850f68a06c | 87 | } |
guesta | 5:1a850f68a06c | 88 | }else{ |
guesta | 5:1a850f68a06c | 89 | TG.pass_val((x_pos_target[0] - x_position),(y_pos_target[0] - y_position),theta); |
kazumayamanaka | 0:a1238c4cd105 | 90 | } |
kazumayamanaka | 0:a1238c4cd105 | 91 | |
guesta | 5:1a850f68a06c | 92 | pid_1.pass_val(data_1.get(),TG.obt_target1(),0.0001,0.0000014,0.0000001); |
guesta | 5:1a850f68a06c | 93 | pid_2.pass_val(data_2.get(),TG.obt_target2(),0.0001,0.0000014,0.0000001); |
guesta | 5:1a850f68a06c | 94 | pid_3.pass_val(data_3.get(),TG.obt_target3(),0.0001,0.0000014,0.0000001); |
guesta | 5:1a850f68a06c | 95 | pid_4.pass_val(data_4.get(),TG.obt_target4(),0.0001,0.0000014,0.0000001); |
kazumayamanaka | 0:a1238c4cd105 | 96 | |
kazumayamanaka | 1:ea880e226e5a | 97 | pid_1.wheel_ctl(PC_9,PC_8); |
kazumayamanaka | 0:a1238c4cd105 | 98 | pid_2.wheel_ctl(PB_14,PB_13); |
kazumayamanaka | 0:a1238c4cd105 | 99 | pid_3.wheel_ctl(PB_5,PB_4); |
kazumayamanaka | 0:a1238c4cd105 | 100 | pid_4.wheel_ctl(PB_7,PB_6); |
kazumayamanaka | 0:a1238c4cd105 | 101 | |
guesta | 2:612efbe94f42 | 102 | if(TG.obt_target1() != 0){ |
guesta | 2:612efbe94f42 | 103 | led1 = 1; |
guesta | 2:612efbe94f42 | 104 | }else{ |
guesta | 2:612efbe94f42 | 105 | led1 = 0; |
guesta | 2:612efbe94f42 | 106 | } |
guesta | 2:612efbe94f42 | 107 | if(TG.obt_target2() != 0){ |
guesta | 2:612efbe94f42 | 108 | led2 = 1; |
guesta | 2:612efbe94f42 | 109 | }else{ |
guesta | 2:612efbe94f42 | 110 | led2 = 0; |
guesta | 2:612efbe94f42 | 111 | } |
guesta | 2:612efbe94f42 | 112 | if(TG.obt_target3() != 0){ |
guesta | 2:612efbe94f42 | 113 | led3 = 1; |
guesta | 2:612efbe94f42 | 114 | }else{ |
guesta | 2:612efbe94f42 | 115 | led3 = 0; |
guesta | 2:612efbe94f42 | 116 | } |
guesta | 2:612efbe94f42 | 117 | if(TG.obt_target4() != 0){ |
guesta | 2:612efbe94f42 | 118 | led4 = 1; |
guesta | 2:612efbe94f42 | 119 | }else{ |
guesta | 2:612efbe94f42 | 120 | led4 = 0; |
guesta | 2:612efbe94f42 | 121 | } |
guesta | 2:612efbe94f42 | 122 | |
guesta | 4:867c2b5cf81a | 123 | printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n%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(),x_position,y_position); |
guesta | 5:1a850f68a06c | 124 | if(auto_mode == false){ |
guesta | 5:1a850f68a06c | 125 | printf("manual\n"); |
guesta | 5:1a850f68a06c | 126 | }else if(auto_mode == true){ |
guesta | 5:1a850f68a06c | 127 | printf("auto\n"); |
guesta | 5:1a850f68a06c | 128 | } |
guesta | 5:1a850f68a06c | 129 | |
kazumayamanaka | 0:a1238c4cd105 | 130 | while(Time.read_us() - timer <= 50 * 1000); |
kazumayamanaka | 0:a1238c4cd105 | 131 | } |
kazumayamanaka | 0:a1238c4cd105 | 132 | } |
kazumayamanaka | 0:a1238c4cd105 | 133 | |
kazumayamanaka | 0:a1238c4cd105 | 134 | //BNOのピン(PB_3,PB_10) |
kazumayamanaka | 0:a1238c4cd105 | 135 | |
kazumayamanaka | 0:a1238c4cd105 | 136 | |
kazumayamanaka | 0:a1238c4cd105 | 137 |