2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@6:e089fda81b74, 2022-01-24 (annotated)

Committer:
guesta
Date:
Mon Jan 24 09:21:43 2022 +0000
Revision:
6:e089fda81b74
Parent:
5:1a850f68a06c
Child:
7:a0375e6dc8ca
add odometry;

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 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 6:e089fda81b74 48 double x_pos_target[] = {0};
guesta 6:e089fda81b74 49 double y_pos_target[] = {0};
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 6:e089fda81b74 67 theta2 = (90 - bno.euler.yaw) * (PI /180);
kazumayamanaka 1:ea880e226e5a 68
kazumayamanaka 0:a1238c4cd105 69 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 70 theta = -(2 * PI - theta);
guesta 6:e089fda81b74 71 //pltheta = theta - 2 * PI;
guesta 6:e089fda81b74 72 }else{
guesta 6:e089fda81b74 73 pltheta = theta;
guesta 6:e089fda81b74 74
kazumayamanaka 0:a1238c4cd105 75 }
guesta 6:e089fda81b74 76 theta = theta - (PI / 4);
kazumayamanaka 0:a1238c4cd105 77
guesta 4:867c2b5cf81a 78 if(theta2 > PI){
guesta 5:1a850f68a06c 79 pltheta = theta2 - 2 * PI;
guesta 4:867c2b5cf81a 80 }else{
guesta 4:867c2b5cf81a 81 pltheta = theta2;
guesta 4:867c2b5cf81a 82 }
guesta 4:867c2b5cf81a 83
guesta 4:867c2b5cf81a 84 get_position(pltheta);
guesta 2:612efbe94f42 85 DS.pass_val(x_component,y_component,r2_num,l2_num);
kazumayamanaka 0:a1238c4cd105 86
guesta 5:1a850f68a06c 87 if(auto_mode == false){
guesta 5:1a850f68a06c 88 if(DS.cal_input() == true){
guesta 5:1a850f68a06c 89 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
guesta 5:1a850f68a06c 90 }else{
guesta 5:1a850f68a06c 91 TG.pass_target(DS.obt_X());
guesta 5:1a850f68a06c 92 }
guesta 5:1a850f68a06c 93 }else{
guesta 5:1a850f68a06c 94 TG.pass_val((x_pos_target[0] - x_position),(y_pos_target[0] - y_position),theta);
kazumayamanaka 0:a1238c4cd105 95 }
kazumayamanaka 0:a1238c4cd105 96
guesta 5:1a850f68a06c 97 pid_1.pass_val(data_1.get(),TG.obt_target1(),0.0001,0.0000014,0.0000001);
guesta 5:1a850f68a06c 98 pid_2.pass_val(data_2.get(),TG.obt_target2(),0.0001,0.0000014,0.0000001);
guesta 5:1a850f68a06c 99 pid_3.pass_val(data_3.get(),TG.obt_target3(),0.0001,0.0000014,0.0000001);
guesta 5:1a850f68a06c 100 pid_4.pass_val(data_4.get(),TG.obt_target4(),0.0001,0.0000014,0.0000001);
kazumayamanaka 0:a1238c4cd105 101
kazumayamanaka 1:ea880e226e5a 102 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 103 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 104 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 105 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 106
guesta 2:612efbe94f42 107 if(TG.obt_target1() != 0){
guesta 2:612efbe94f42 108 led1 = 1;
guesta 2:612efbe94f42 109 }else{
guesta 2:612efbe94f42 110 led1 = 0;
guesta 2:612efbe94f42 111 }
guesta 2:612efbe94f42 112 if(TG.obt_target2() != 0){
guesta 2:612efbe94f42 113 led2 = 1;
guesta 2:612efbe94f42 114 }else{
guesta 2:612efbe94f42 115 led2 = 0;
guesta 2:612efbe94f42 116 }
guesta 2:612efbe94f42 117 if(TG.obt_target3() != 0){
guesta 2:612efbe94f42 118 led3 = 1;
guesta 2:612efbe94f42 119 }else{
guesta 2:612efbe94f42 120 led3 = 0;
guesta 2:612efbe94f42 121 }
guesta 2:612efbe94f42 122 if(TG.obt_target4() != 0){
guesta 2:612efbe94f42 123 led4 = 1;
guesta 2:612efbe94f42 124 }else{
guesta 2:612efbe94f42 125 led4 = 0;
guesta 2:612efbe94f42 126 }
guesta 2:612efbe94f42 127
guesta 6:e089fda81b74 128 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 129 printf("%lf,%lf\n",x_position,y_position);
guesta 6:e089fda81b74 130 /*if(auto_mode == false){
guesta 5:1a850f68a06c 131 printf("manual\n");
guesta 5:1a850f68a06c 132 }else if(auto_mode == true){
guesta 5:1a850f68a06c 133 printf("auto\n");
guesta 6:e089fda81b74 134 }*/
guesta 5:1a850f68a06c 135
kazumayamanaka 0:a1238c4cd105 136 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 137 }
kazumayamanaka 0:a1238c4cd105 138 }
kazumayamanaka 0:a1238c4cd105 139
kazumayamanaka 0:a1238c4cd105 140 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 141
kazumayamanaka 0:a1238c4cd105 142
kazumayamanaka 0:a1238c4cd105 143