2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@5:1a850f68a06c, 2022-01-23 (annotated)

Committer:
guesta
Date:
Sun Jan 23 13:31:36 2022 +0000
Revision:
5:1a850f68a06c
Parent:
4:867c2b5cf81a
Child:
6:e089fda81b74
add go to target_position;

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