2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@8:3d6b1f78f89c, 2022-01-29 (annotated)

Committer:
guesta
Date:
Sat Jan 29 14:47:48 2022 +0000
Revision:
8:3d6b1f78f89c
Parent:
7:a0375e6dc8ca
Child:
9:569c0f55dd9b
add id cmd

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