2022_gorobo_Ateam / gourobo2022_undercarriage_new

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?

UserRevisionLine numberNew 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)