2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@9:569c0f55dd9b, 2022-02-03 (annotated)

Committer:
guesta
Date:
Thu Feb 03 13:07:37 2022 +0000
Revision:
9:569c0f55dd9b
Parent:
8:3d6b1f78f89c
Child:
10:ad8fced7d6b6
add regulation;

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 8:3d6b1f78f89c 16 ScrpSlave scrp(PC_12,PD_2,PH_1,1);
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
guesta 8:3d6b1f78f89c 28 PIDco pid_angle;
guesta 8:3d6b1f78f89c 29
kazumayamanaka 0:a1238c4cd105 30 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 31 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 32 DUALSHOCKco DS;
kazumayamanaka 0:a1238c4cd105 33
guesta 2:612efbe94f42 34 //sbdbt sb(PA_0,PA_1);
guesta 2:612efbe94f42 35
guesta 2:612efbe94f42 36 DigitalOut led1(PA_10);
guesta 2:612efbe94f42 37 DigitalOut led2(PB_15);
guesta 2:612efbe94f42 38 DigitalOut led3(PB_2);
guesta 2:612efbe94f42 39 DigitalOut led4(PC_6);
kazumayamanaka 0:a1238c4cd105 40
kazumayamanaka 0:a1238c4cd105 41 Timer Time;
kazumayamanaka 0:a1238c4cd105 42 double timer;
kazumayamanaka 0:a1238c4cd105 43 double theta;
kazumayamanaka 0:a1238c4cd105 44 double X,Y;
guesta 7:a0375e6dc8ca 45 double auto_x_component;
guesta 7:a0375e6dc8ca 46 double auto_y_component;
guesta 8:3d6b1f78f89c 47 bool position_arrival = false;
guesta 8:3d6b1f78f89c 48 bool angle_arrival = true;
guesta 9:569c0f55dd9b 49 double regulation = 0.5;
kazumayamanaka 0:a1238c4cd105 50
guesta 4:867c2b5cf81a 51 //for odometry
guesta 4:867c2b5cf81a 52 double theta2;
guesta 4:867c2b5cf81a 53 double pltheta;
guesta 4:867c2b5cf81a 54
guesta 9:569c0f55dd9b 55 //target
guesta 9:569c0f55dd9b 56 double x_pos_target[] = {0};
guesta 9:569c0f55dd9b 57 double y_pos_target[] = {30};
guesta 8:3d6b1f78f89c 58 double angle_target[] = {10};
guesta 7:a0375e6dc8ca 59
guesta 7:a0375e6dc8ca 60 double max_mini(double a){
guesta 7:a0375e6dc8ca 61 if(a > 450){
guesta 7:a0375e6dc8ca 62 a = 450;
guesta 7:a0375e6dc8ca 63 }else if(a < -450){
guesta 7:a0375e6dc8ca 64 a = -450;
guesta 7:a0375e6dc8ca 65 }else if(a == 0){
guesta 7:a0375e6dc8ca 66 a = 0;
guesta 7:a0375e6dc8ca 67 }
guesta 7:a0375e6dc8ca 68 return a;
guesta 7:a0375e6dc8ca 69 }
guesta 4:867c2b5cf81a 70
kazumayamanaka 0:a1238c4cd105 71 int main(){
guesta 7:a0375e6dc8ca 72 bno.reset();
kazumayamanaka 0:a1238c4cd105 73 Time.start();
guesta 2:612efbe94f42 74 TG.pass_val(0,0,0);
guesta 2:612efbe94f42 75 scrp.addCMD(1,getLstick_x);
guesta 2:612efbe94f42 76 scrp.addCMD(2,getLstick_y);
guesta 2:612efbe94f42 77 scrp.addCMD(3,getL2);
guesta 2:612efbe94f42 78 scrp.addCMD(4,getR2);
guesta 9:569c0f55dd9b 79 scrp.addCMD(50,change_mode);
guesta 2:612efbe94f42 80
kazumayamanaka 0:a1238c4cd105 81
kazumayamanaka 0:a1238c4cd105 82 while(1){
kazumayamanaka 0:a1238c4cd105 83 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 84 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 85 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 86
guesta 9:569c0f55dd9b 87
kazumayamanaka 1:ea880e226e5a 88 theta = bno.euler.yaw * (PI / 180);
guesta 6:e089fda81b74 89 theta2 = (90 - bno.euler.yaw) * (PI /180);
kazumayamanaka 1:ea880e226e5a 90
kazumayamanaka 0:a1238c4cd105 91 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 92 theta = -(2 * PI - theta);
guesta 7:a0375e6dc8ca 93
guesta 6:e089fda81b74 94 }else{
kazumayamanaka 0:a1238c4cd105 95 }
guesta 6:e089fda81b74 96 theta = theta - (PI / 4);
kazumayamanaka 0:a1238c4cd105 97
guesta 4:867c2b5cf81a 98 if(theta2 > PI){
guesta 5:1a850f68a06c 99 pltheta = theta2 - 2 * PI;
guesta 4:867c2b5cf81a 100 }else{
guesta 4:867c2b5cf81a 101 pltheta = theta2;
guesta 4:867c2b5cf81a 102 }
guesta 4:867c2b5cf81a 103
guesta 4:867c2b5cf81a 104 get_position(pltheta);
guesta 9:569c0f55dd9b 105 if(l1 == 0){
guesta 9:569c0f55dd9b 106 DS.pass_val(x_component,y_component,r2_num,l2_num);
guesta 9:569c0f55dd9b 107 }else{
guesta 9:569c0f55dd9b 108 DS.pass_val(x_component * regulation,y_component * regulation,r2_num * regulation,l2_num * regulation);
guesta 9:569c0f55dd9b 109 }
guesta 9:569c0f55dd9b 110 //printf("%d\n",y_component);
kazumayamanaka 0:a1238c4cd105 111
guesta 5:1a850f68a06c 112 if(auto_mode == false){
guesta 5:1a850f68a06c 113 if(DS.cal_input() == true){
guesta 5:1a850f68a06c 114 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
guesta 5:1a850f68a06c 115 }else{
guesta 5:1a850f68a06c 116 TG.pass_target(DS.obt_X());
guesta 5:1a850f68a06c 117 }
guesta 5:1a850f68a06c 118 }else{
guesta 8:3d6b1f78f89c 119 //printf("%lf\n",bno.euler.yaw);
guesta 9:569c0f55dd9b 120 if(position_arrival == false){
guesta 8:3d6b1f78f89c 121 auto_x_component = max_mini((x_pos_target[0] - x_position) * 150);
guesta 8:3d6b1f78f89c 122 auto_y_component = max_mini((y_pos_target[0] - y_position) * 150);
guesta 8:3d6b1f78f89c 123 TG.pass_val(auto_y_component,auto_x_component,theta);
guesta 9:569c0f55dd9b 124 //printf("go to target\n");
guesta 9:569c0f55dd9b 125 if((x_pos_target[0] - x_position) <= 0.5 && (x_pos_target[0] - x_position) >= -0.5 && (y_pos_target[0] - y_position) <= 0.5 && (y_pos_target[0] - y_position) >= -0.5){
guesta 8:3d6b1f78f89c 126 position_arrival = true;
guesta 8:3d6b1f78f89c 127 TG.pass_val(0,0,theta);
guesta 8:3d6b1f78f89c 128 printf("arrival_position\n");
guesta 8:3d6b1f78f89c 129 }
guesta 8:3d6b1f78f89c 130 }else{
guesta 8:3d6b1f78f89c 131 if(angle_arrival == false){
guesta 9:569c0f55dd9b 132 pid_angle.angle_ctl(bno.euler.yaw,angle_target[0],0.004,0.0007,0);
guesta 9:569c0f55dd9b 133 TG.pass_target(pid_angle.output);
guesta 9:569c0f55dd9b 134 if(angle_target[0] - bno.euler.yaw <= 0.1 && angle_target[0] - bno.euler.yaw >= -0.1){
guesta 8:3d6b1f78f89c 135 angle_arrival = true;
guesta 8:3d6b1f78f89c 136 TG.pass_val(0,0,theta);
guesta 8:3d6b1f78f89c 137 printf("arrival_angle\n");
guesta 8:3d6b1f78f89c 138 }
guesta 8:3d6b1f78f89c 139 }
guesta 9:569c0f55dd9b 140 }
kazumayamanaka 0:a1238c4cd105 141 }
kazumayamanaka 0:a1238c4cd105 142
guesta 7:a0375e6dc8ca 143 pid_1.pass_val(data_1.get(),TG.obt_target1(),0.00007,0.000001,0.00000014);
guesta 7:a0375e6dc8ca 144 pid_2.pass_val(data_2.get(),TG.obt_target2(),0.00007,0.000001,0.00000014);
guesta 7:a0375e6dc8ca 145 pid_3.pass_val(data_3.get(),TG.obt_target3(),0.00007,0.000001,0.00000014);
guesta 7:a0375e6dc8ca 146 pid_4.pass_val(data_4.get(),TG.obt_target4(),0.00007,0.000001,0.00000014);
kazumayamanaka 0:a1238c4cd105 147
kazumayamanaka 1:ea880e226e5a 148 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 149 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 150 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 151 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 152
guesta 2:612efbe94f42 153 if(TG.obt_target1() != 0){
guesta 2:612efbe94f42 154 led1 = 1;
guesta 2:612efbe94f42 155 }else{
guesta 2:612efbe94f42 156 led1 = 0;
guesta 2:612efbe94f42 157 }
guesta 2:612efbe94f42 158 if(TG.obt_target2() != 0){
guesta 2:612efbe94f42 159 led2 = 1;
guesta 2:612efbe94f42 160 }else{
guesta 2:612efbe94f42 161 led2 = 0;
guesta 2:612efbe94f42 162 }
guesta 2:612efbe94f42 163 if(TG.obt_target3() != 0){
guesta 2:612efbe94f42 164 led3 = 1;
guesta 2:612efbe94f42 165 }else{
guesta 2:612efbe94f42 166 led3 = 0;
guesta 2:612efbe94f42 167 }
guesta 2:612efbe94f42 168 if(TG.obt_target4() != 0){
guesta 2:612efbe94f42 169 led4 = 1;
guesta 2:612efbe94f42 170 }else{
guesta 2:612efbe94f42 171 led4 = 0;
guesta 2:612efbe94f42 172 }
guesta 2:612efbe94f42 173
guesta 9:569c0f55dd9b 174 //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());
guesta 6:e089fda81b74 175 printf("%lf,%lf\n",x_position,y_position);
guesta 6:e089fda81b74 176 /*if(auto_mode == false){
guesta 5:1a850f68a06c 177 printf("manual\n");
guesta 5:1a850f68a06c 178 }else if(auto_mode == true){
guesta 5:1a850f68a06c 179 printf("auto\n");
guesta 6:e089fda81b74 180 }*/
guesta 5:1a850f68a06c 181
kazumayamanaka 0:a1238c4cd105 182 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 183 }
kazumayamanaka 0:a1238c4cd105 184 }
kazumayamanaka 0:a1238c4cd105 185
guesta 7:a0375e6dc8ca 186 //BNOのピン(PB_3,PB_10)