2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
main.cpp@13:369f4abc1f36, 2022-04-08 (annotated)
- Committer:
- guesta
- Date:
- Fri Apr 08 09:14:20 2022 +0000
- Revision:
- 13:369f4abc1f36
- Parent:
- 12:894e5ac49810
2022/04/08
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 | 10:ad8fced7d6b6 | 16 | //---------------初期設定--------------- |
guesta | 10:ad8fced7d6b6 | 17 | |
guesta | 8:3d6b1f78f89c | 18 | ScrpSlave scrp(PC_12,PD_2,PH_1,1); |
kazumayamanaka | 0:a1238c4cd105 | 19 | |
kazumayamanaka | 0:a1238c4cd105 | 20 | RotaryInc data_1(PA_14,PA_15,0); |
kazumayamanaka | 0:a1238c4cd105 | 21 | RotaryInc data_2(PA_12,PC_5,0); |
kazumayamanaka | 0:a1238c4cd105 | 22 | RotaryInc data_3(PC_0,PC_1,0); |
kazumayamanaka | 0:a1238c4cd105 | 23 | RotaryInc data_4(PC_2,PC_3,0); |
kazumayamanaka | 0:a1238c4cd105 | 24 | |
kazumayamanaka | 0:a1238c4cd105 | 25 | PIDco pid_1; |
kazumayamanaka | 0:a1238c4cd105 | 26 | PIDco pid_2; |
kazumayamanaka | 0:a1238c4cd105 | 27 | PIDco pid_3; |
kazumayamanaka | 0:a1238c4cd105 | 28 | PIDco pid_4; |
kazumayamanaka | 0:a1238c4cd105 | 29 | |
guesta | 8:3d6b1f78f89c | 30 | PIDco pid_angle; |
guesta | 10:ad8fced7d6b6 | 31 | PIDco x_pos; |
guesta | 10:ad8fced7d6b6 | 32 | PIDco y_pos; |
guesta | 8:3d6b1f78f89c | 33 | |
kazumayamanaka | 0:a1238c4cd105 | 34 | TARGETco TG; |
kazumayamanaka | 0:a1238c4cd105 | 35 | BNO055 bno(SDA,SCL); |
kazumayamanaka | 0:a1238c4cd105 | 36 | DUALSHOCKco DS; |
guesta | 11:264f992664b0 | 37 | DUALSHOCKco DS2; |
kazumayamanaka | 0:a1238c4cd105 | 38 | |
guesta | 2:612efbe94f42 | 39 | //sbdbt sb(PA_0,PA_1); |
guesta | 2:612efbe94f42 | 40 | |
guesta | 2:612efbe94f42 | 41 | DigitalOut led1(PA_10); |
guesta | 2:612efbe94f42 | 42 | DigitalOut led2(PB_15); |
guesta | 2:612efbe94f42 | 43 | DigitalOut led3(PB_2); |
guesta | 2:612efbe94f42 | 44 | DigitalOut led4(PC_6); |
kazumayamanaka | 0:a1238c4cd105 | 45 | |
kazumayamanaka | 0:a1238c4cd105 | 46 | Timer Time; |
guesta | 11:264f992664b0 | 47 | long double timer; |
kazumayamanaka | 0:a1238c4cd105 | 48 | double theta; |
kazumayamanaka | 0:a1238c4cd105 | 49 | double X,Y; |
guesta | 7:a0375e6dc8ca | 50 | double auto_x_component; |
guesta | 7:a0375e6dc8ca | 51 | double auto_y_component; |
guesta | 8:3d6b1f78f89c | 52 | bool position_arrival = false; |
guesta | 10:ad8fced7d6b6 | 53 | bool angle_arrival = false; |
guesta | 10:ad8fced7d6b6 | 54 | double regulation; |
guesta | 11:264f992664b0 | 55 | bool wheel_limit; |
kazumayamanaka | 0:a1238c4cd105 | 56 | |
guesta | 4:867c2b5cf81a | 57 | //for odometry |
guesta | 4:867c2b5cf81a | 58 | double theta2; |
guesta | 4:867c2b5cf81a | 59 | double pltheta; |
guesta | 4:867c2b5cf81a | 60 | |
guesta | 9:569c0f55dd9b | 61 | //target |
guesta | 11:264f992664b0 | 62 | double x_pos_target[] = {81.72,81.72}; |
guesta | 11:264f992664b0 | 63 | double y_pos_target[] = {-90.47,90.47}; |
guesta | 12:894e5ac49810 | 64 | double angle_target[] = {-19.0,19.0}; |
guesta | 10:ad8fced7d6b6 | 65 | |
guesta | 10:ad8fced7d6b6 | 66 | double robot_angle; |
guesta | 7:a0375e6dc8ca | 67 | |
guesta | 11:264f992664b0 | 68 | double max_mini(double a) |
guesta | 11:264f992664b0 | 69 | { |
guesta | 11:264f992664b0 | 70 | if(a > 450) { |
guesta | 7:a0375e6dc8ca | 71 | a = 450; |
guesta | 11:264f992664b0 | 72 | } else if(a < -450) { |
guesta | 7:a0375e6dc8ca | 73 | a = -450; |
guesta | 11:264f992664b0 | 74 | } else if(a == 0) { |
guesta | 7:a0375e6dc8ca | 75 | a = 0; |
guesta | 7:a0375e6dc8ca | 76 | } |
guesta | 7:a0375e6dc8ca | 77 | return a; |
guesta | 11:264f992664b0 | 78 | } |
guesta | 11:264f992664b0 | 79 | |
guesta | 11:264f992664b0 | 80 | void rotate() |
guesta | 11:264f992664b0 | 81 | { |
guesta | 11:264f992664b0 | 82 | if(l1 == 1) { |
guesta | 11:264f992664b0 | 83 | regulation = 0.3; |
guesta | 11:264f992664b0 | 84 | } else { |
guesta | 12:894e5ac49810 | 85 | regulation = 1.0; |
guesta | 11:264f992664b0 | 86 | } |
guesta | 11:264f992664b0 | 87 | |
guesta | 12:894e5ac49810 | 88 | if(r1 == 1) { |
guesta | 12:894e5ac49810 | 89 | wheel_limit = 0.0; |
guesta | 12:894e5ac49810 | 90 | } else { |
guesta | 12:894e5ac49810 | 91 | wheel_limit = 1.0; |
guesta | 12:894e5ac49810 | 92 | } |
guesta | 12:894e5ac49810 | 93 | |
guesta | 12:894e5ac49810 | 94 | pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,0.0,0); |
guesta | 11:264f992664b0 | 95 | pid_2.pass_val(data_2.get(),TG.obt_target2() * regulation,0.00005,0.0,0); |
guesta | 11:264f992664b0 | 96 | pid_3.pass_val(data_3.get(),TG.obt_target3() * regulation,0.00005,0.0,0); |
guesta | 12:894e5ac49810 | 97 | pid_4.pass_val(data_4.get(),TG.obt_target4() * regulation,0.00005,0.0,0); |
guesta | 11:264f992664b0 | 98 | |
guesta | 11:264f992664b0 | 99 | pid_1.wheel_ctl(PC_9,PC_8,wheel_limit); |
guesta | 11:264f992664b0 | 100 | pid_2.wheel_ctl(PB_14,PB_13,wheel_limit); |
guesta | 11:264f992664b0 | 101 | pid_3.wheel_ctl(PB_5,PB_4,wheel_limit); |
guesta | 11:264f992664b0 | 102 | pid_4.wheel_ctl(PB_7,PB_6,wheel_limit); |
guesta | 11:264f992664b0 | 103 | |
guesta | 11:264f992664b0 | 104 | if(TG.obt_target1() != 0) { |
guesta | 11:264f992664b0 | 105 | led1 = 1; |
guesta | 11:264f992664b0 | 106 | } else { |
guesta | 11:264f992664b0 | 107 | led1 = 0; |
guesta | 11:264f992664b0 | 108 | } |
guesta | 11:264f992664b0 | 109 | if(TG.obt_target2() != 0) { |
guesta | 11:264f992664b0 | 110 | led2 = 1; |
guesta | 11:264f992664b0 | 111 | } else { |
guesta | 11:264f992664b0 | 112 | led2 = 0; |
guesta | 11:264f992664b0 | 113 | } |
guesta | 11:264f992664b0 | 114 | if(TG.obt_target3() != 0) { |
guesta | 11:264f992664b0 | 115 | led3 = 1; |
guesta | 11:264f992664b0 | 116 | } else { |
guesta | 11:264f992664b0 | 117 | led3 = 0; |
guesta | 11:264f992664b0 | 118 | } |
guesta | 11:264f992664b0 | 119 | if(TG.obt_target4() != 0) { |
guesta | 11:264f992664b0 | 120 | led4 = 1; |
guesta | 11:264f992664b0 | 121 | } else { |
guesta | 11:264f992664b0 | 122 | led4 = 0; |
guesta | 11:264f992664b0 | 123 | } |
guesta | 11:264f992664b0 | 124 | } |
guesta | 11:264f992664b0 | 125 | |
guesta | 12:894e5ac49810 | 126 | void VectorDrection4(int x,int y,double theta) |
guesta | 12:894e5ac49810 | 127 | { |
guesta | 11:264f992664b0 | 128 | x = x - 64; |
guesta | 11:264f992664b0 | 129 | y = y - 64; |
guesta | 11:264f992664b0 | 130 | //printf("%d %d\n",x,y); |
guesta | 12:894e5ac49810 | 131 | if(y > -abs(x)) { //上 |
guesta | 11:264f992664b0 | 132 | DS2.pass_val(64,y + 64,0,0); |
guesta | 11:264f992664b0 | 133 | } |
guesta | 12:894e5ac49810 | 134 | if(y < abs(x)) { //下 |
guesta | 11:264f992664b0 | 135 | DS2.pass_val(64,y + 64,0,0); |
guesta | 11:264f992664b0 | 136 | } |
guesta | 12:894e5ac49810 | 137 | if(x < -abs(y)) { //左 |
guesta | 11:264f992664b0 | 138 | DS2.pass_val(x + 64,64,0,0); |
guesta | 11:264f992664b0 | 139 | } |
guesta | 12:894e5ac49810 | 140 | if(x > abs(y)) { //右 |
guesta | 11:264f992664b0 | 141 | DS2.pass_val(x + 64,64,0,0); |
guesta | 11:264f992664b0 | 142 | } |
guesta | 12:894e5ac49810 | 143 | if(y == 0 && x == 0) { |
guesta | 11:264f992664b0 | 144 | DS2.pass_val(64,64,0,0); |
guesta | 11:264f992664b0 | 145 | } |
guesta | 11:264f992664b0 | 146 | DS2.cal_input(); |
guesta | 11:264f992664b0 | 147 | //printf("%lf %lf\n",DS2.obt_X(),DS2.obt_Y()); |
guesta | 11:264f992664b0 | 148 | TG.pass_val(DS2.obt_X(),DS2.obt_Y(),theta); |
guesta | 11:264f992664b0 | 149 | rotate(); |
guesta | 11:264f992664b0 | 150 | } |
guesta | 11:264f992664b0 | 151 | |
guesta | 10:ad8fced7d6b6 | 152 | //------------------------------------- |
guesta | 4:867c2b5cf81a | 153 | |
guesta | 11:264f992664b0 | 154 | int main() |
guesta | 11:264f992664b0 | 155 | { |
guesta | 7:a0375e6dc8ca | 156 | bno.reset(); |
kazumayamanaka | 0:a1238c4cd105 | 157 | Time.start(); |
guesta | 2:612efbe94f42 | 158 | TG.pass_val(0,0,0); |
guesta | 2:612efbe94f42 | 159 | scrp.addCMD(1,getLstick_x); |
guesta | 2:612efbe94f42 | 160 | scrp.addCMD(2,getLstick_y); |
guesta | 2:612efbe94f42 | 161 | scrp.addCMD(3,getL2); |
guesta | 2:612efbe94f42 | 162 | scrp.addCMD(4,getR2); |
guesta | 11:264f992664b0 | 163 | scrp.addCMD(5,getL1); |
guesta | 11:264f992664b0 | 164 | scrp.addCMD(6,getCircle); |
guesta | 11:264f992664b0 | 165 | scrp.addCMD(7,getZone); |
guesta | 11:264f992664b0 | 166 | scrp.addCMD(8,getR1); |
guesta | 11:264f992664b0 | 167 | scrp.addCMD(9,getRstick_x); |
guesta | 11:264f992664b0 | 168 | scrp.addCMD(10,getRstick_y); |
guesta | 9:569c0f55dd9b | 169 | scrp.addCMD(50,change_mode); |
guesta | 11:264f992664b0 | 170 | scrp.addCMD(51,getStop); |
guesta | 2:612efbe94f42 | 171 | |
guesta | 7:a0375e6dc8ca | 172 | |
guesta | 11:264f992664b0 | 173 | while(1) { |
guesta | 11:264f992664b0 | 174 | TG.pass_val(0,0,theta); |
guesta | 11:264f992664b0 | 175 | if(stop == 0) { |
guesta | 11:264f992664b0 | 176 | timer = Time.read_ms(); |
guesta | 11:264f992664b0 | 177 | bno.setmode(OPERATION_MODE_IMUPLUS); |
guesta | 11:264f992664b0 | 178 | bno.get_angles(); |
guesta | 12:894e5ac49810 | 179 | if(auto_mode == false) { |
guesta | 12:894e5ac49810 | 180 | if(redZone == true) { |
guesta | 11:264f992664b0 | 181 | theta = (bno.euler.yaw - 90) * (PI / 180); |
guesta | 11:264f992664b0 | 182 | theta2 = (bno.euler.yaw - 180) * (PI /180); |
guesta | 12:894e5ac49810 | 183 | } else { |
guesta | 11:264f992664b0 | 184 | theta = (bno.euler.yaw + 90) * (PI / 180); |
guesta | 11:264f992664b0 | 185 | theta2 = bno.euler.yaw * (PI /180); |
guesta | 8:3d6b1f78f89c | 186 | } |
guesta | 12:894e5ac49810 | 187 | } else { |
guesta | 11:264f992664b0 | 188 | theta = bno.euler.yaw * (PI / 180); |
guesta | 11:264f992664b0 | 189 | theta2 = (90 - bno.euler.yaw) * (PI /180); |
guesta | 11:264f992664b0 | 190 | } |
guesta | 11:264f992664b0 | 191 | |
guesta | 11:264f992664b0 | 192 | if(theta > PI) { |
guesta | 11:264f992664b0 | 193 | theta = -(2 * PI - theta); |
guesta | 11:264f992664b0 | 194 | |
guesta | 11:264f992664b0 | 195 | } else { |
guesta | 11:264f992664b0 | 196 | } |
guesta | 11:264f992664b0 | 197 | theta = theta - (PI / 4); |
guesta | 11:264f992664b0 | 198 | |
guesta | 11:264f992664b0 | 199 | if(theta2 > PI) { |
guesta | 11:264f992664b0 | 200 | pltheta = theta2 - 2 * PI; |
guesta | 11:264f992664b0 | 201 | } else { |
guesta | 11:264f992664b0 | 202 | pltheta = theta2; |
guesta | 11:264f992664b0 | 203 | } |
guesta | 11:264f992664b0 | 204 | |
guesta | 12:894e5ac49810 | 205 | |
guesta | 11:264f992664b0 | 206 | DS.pass_val(x_component,y_component,r2_num,l2_num); |
guesta | 11:264f992664b0 | 207 | |
guesta | 11:264f992664b0 | 208 | if(auto_mode == false) { |
guesta | 11:264f992664b0 | 209 | DS.cal_input(); |
guesta | 11:264f992664b0 | 210 | TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); |
guesta | 11:264f992664b0 | 211 | rotate(); |
guesta | 11:264f992664b0 | 212 | DS.cal_input(); |
guesta | 11:264f992664b0 | 213 | TG.pass_target(-1 * DS.obt_rotate()); |
guesta | 11:264f992664b0 | 214 | rotate(); |
guesta | 11:264f992664b0 | 215 | VectorDrection4(l_x_component,l_y_component,theta); |
guesta | 11:264f992664b0 | 216 | } else { |
guesta | 12:894e5ac49810 | 217 | get_position(pltheta); |
guesta | 11:264f992664b0 | 218 | if(auto_move == 1) { |
guesta | 11:264f992664b0 | 219 | if(position_arrival == false) { |
guesta | 12:894e5ac49810 | 220 | x_pos.pid_ctl(x_position,x_pos_target[redZone],20.0,0.007,0.0005); |
guesta | 12:894e5ac49810 | 221 | y_pos.pid_ctl(y_position,y_pos_target[redZone],20.0,0.007,0.0005); |
guesta | 11:264f992664b0 | 222 | auto_x_component = max_mini(x_pos.output); |
guesta | 11:264f992664b0 | 223 | auto_y_component = max_mini(y_pos.output); |
guesta | 11:264f992664b0 | 224 | TG.pass_val(auto_y_component,auto_x_component,theta); |
guesta | 11:264f992664b0 | 225 | rotate(); |
guesta | 11:264f992664b0 | 226 | //printf("go to target\n"); |
guesta | 11:264f992664b0 | 227 | if((x_pos_target[redZone] - x_position) <= 0.5 && (x_pos_target[redZone] - x_position) >= -0.5 && (y_pos_target[redZone] - y_position) <= 0.5 && (y_pos_target[redZone] - y_position) >= -0.5) { |
guesta | 11:264f992664b0 | 228 | position_arrival = true; |
guesta | 11:264f992664b0 | 229 | TG.pass_val(64,64,theta); |
guesta | 11:264f992664b0 | 230 | rotate(); |
guesta | 11:264f992664b0 | 231 | printf("arrival_position\n"); |
guesta | 11:264f992664b0 | 232 | } |
guesta | 11:264f992664b0 | 233 | } else { |
guesta | 12:894e5ac49810 | 234 | if(r1 == 1) { |
guesta | 12:894e5ac49810 | 235 | TG.pass_val(0,0,theta); |
guesta | 12:894e5ac49810 | 236 | rotate(); |
guesta | 12:894e5ac49810 | 237 | TG.pass_target(0); |
guesta | 11:264f992664b0 | 238 | rotate(); |
guesta | 12:894e5ac49810 | 239 | } else { |
guesta | 12:894e5ac49810 | 240 | if(angle_arrival == false) { |
guesta | 12:894e5ac49810 | 241 | //printf("go to angle_target\n"); |
guesta | 12:894e5ac49810 | 242 | if(bno.euler.yaw >= 0 && bno.euler.yaw <= 180) { |
guesta | 12:894e5ac49810 | 243 | robot_angle = bno.euler.yaw; |
guesta | 12:894e5ac49810 | 244 | } else { |
guesta | 12:894e5ac49810 | 245 | robot_angle = bno.euler.yaw - 360; |
guesta | 12:894e5ac49810 | 246 | } |
guesta | 12:894e5ac49810 | 247 | pid_angle.pid_ctl(robot_angle,angle_target[redZone],6.0,0.007,0.0001); |
guesta | 12:894e5ac49810 | 248 | printf("%lf,%lf\n",pid_angle.output,robot_angle); |
guesta | 12:894e5ac49810 | 249 | TG.pass_target(-1 * pid_angle.output); |
guesta | 11:264f992664b0 | 250 | rotate(); |
guesta | 12:894e5ac49810 | 251 | if(angle_target[redZone] - robot_angle <= 1 && angle_target[redZone] - robot_angle >= -1) { |
guesta | 12:894e5ac49810 | 252 | angle_arrival = true; |
guesta | 12:894e5ac49810 | 253 | TG.pass_val(64,64,theta); |
guesta | 12:894e5ac49810 | 254 | rotate(); |
guesta | 12:894e5ac49810 | 255 | TG.pass_target(0); |
guesta | 12:894e5ac49810 | 256 | rotate(); |
guesta | 12:894e5ac49810 | 257 | //printf("arrival_angle\n"); |
guesta | 12:894e5ac49810 | 258 | } |
guesta | 11:264f992664b0 | 259 | } |
guesta | 11:264f992664b0 | 260 | } |
guesta | 8:3d6b1f78f89c | 261 | } |
guesta | 8:3d6b1f78f89c | 262 | } |
guesta | 9:569c0f55dd9b | 263 | } |
guesta | 11:264f992664b0 | 264 | |
guesta | 11:264f992664b0 | 265 | while(Time.read_ms() - timer <= 50); |
guesta | 12:894e5ac49810 | 266 | if(Time.read_ms() > 2000000){ |
guesta | 12:894e5ac49810 | 267 | Time.reset(); |
guesta | 12:894e5ac49810 | 268 | } |
guesta | 12:894e5ac49810 | 269 | } else if(stop == 1) { |
guesta | 11:264f992664b0 | 270 | TG.pass_val(0,0,theta); |
guesta | 11:264f992664b0 | 271 | rotate(); |
guesta | 11:264f992664b0 | 272 | TG.pass_target(0); |
guesta | 11:264f992664b0 | 273 | rotate(); |
guesta | 12:894e5ac49810 | 274 | } else if(stop == 3) { |
guesta | 12:894e5ac49810 | 275 | NVIC_SystemReset(); |
guesta | 2:612efbe94f42 | 276 | } |
guesta | 11:264f992664b0 | 277 | } |
kazumayamanaka | 0:a1238c4cd105 | 278 | } |
kazumayamanaka | 0:a1238c4cd105 | 279 | |
guesta | 7:a0375e6dc8ca | 280 | //BNOのピン(PB_3,PB_10) |