kazuma yamanaka / 2022_Ateam_MOTORprogram

自己位置推定機能を追加

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@7:a0b9b6e71e41, 2022-02-23 (annotated)

Committer:
kazumayamanaka
Date:
Wed Feb 23 03:03:36 2022 +0000
Revision:
7:a0b9b6e71e41
Parent:
2:e189d55ef292
Child:
8:f2407caf81ba
automatic and manual control of wheels

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 2:e189d55ef292 6 #include "Odmetry.hpp"
kazumayamanaka 0:a1238c4cd105 7 #include "sbdbt.hpp"
kazumayamanaka 0:a1238c4cd105 8 #include "BNO055.h"
kazumayamanaka 0:a1238c4cd105 9 #define SDA D3
kazumayamanaka 0:a1238c4cd105 10 #define SCL D6
kazumayamanaka 0:a1238c4cd105 11 #define PI 3.1415926535897
kazumayamanaka 0:a1238c4cd105 12
kazumayamanaka 0:a1238c4cd105 13 Serial pc(USBTX,USBRX);
kazumayamanaka 0:a1238c4cd105 14
kazumayamanaka 0:a1238c4cd105 15 RotaryInc data_1(PA_14,PA_15,0);
kazumayamanaka 0:a1238c4cd105 16 RotaryInc data_2(PA_12,PC_5,0);
kazumayamanaka 0:a1238c4cd105 17 RotaryInc data_3(PC_0,PC_1,0);
kazumayamanaka 0:a1238c4cd105 18 RotaryInc data_4(PC_2,PC_3,0);
kazumayamanaka 0:a1238c4cd105 19
kazumayamanaka 2:e189d55ef292 20 RotaryInc MW_1(PA_13,PC_4,0);
kazumayamanaka 2:e189d55ef292 21 RotaryInc MW_2(PC_10,PC_11,0);
kazumayamanaka 2:e189d55ef292 22 RotaryInc MW_3(PA_7,PA_6,0);
kazumayamanaka 2:e189d55ef292 23 RotaryInc MW_4(PA_8,PA_9,0);
kazumayamanaka 2:e189d55ef292 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
kazumayamanaka 0:a1238c4cd105 30 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 31 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 32 DUALSHOCKco DS;
kazumayamanaka 2:e189d55ef292 33 Odmetry odmetry;
kazumayamanaka 0:a1238c4cd105 34 sbdbt sb(PA_0,PA_1);
kazumayamanaka 0:a1238c4cd105 35
kazumayamanaka 0:a1238c4cd105 36 Timer Time;
kazumayamanaka 7:a0b9b6e71e41 37 bool mode=false;
kazumayamanaka 0:a1238c4cd105 38 double timer;
kazumayamanaka 0:a1238c4cd105 39 double theta;
kazumayamanaka 7:a0b9b6e71e41 40 double automaticsX(double PosX);
kazumayamanaka 7:a0b9b6e71e41 41 double automaticsY(double PosY);
kazumayamanaka 7:a0b9b6e71e41 42 double automaticsTHETA(double THETA);
kazumayamanaka 7:a0b9b6e71e41 43 void automatics();
kazumayamanaka 0:a1238c4cd105 44
kazumayamanaka 0:a1238c4cd105 45 int main(){
kazumayamanaka 0:a1238c4cd105 46 Time.start();
kazumayamanaka 7:a0b9b6e71e41 47 bno.reset();
kazumayamanaka 2:e189d55ef292 48
kazumayamanaka 0:a1238c4cd105 49 while(1){
kazumayamanaka 0:a1238c4cd105 50 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 51 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 52 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 53
kazumayamanaka 7:a0b9b6e71e41 54 if(sb.circle()!=0){
kazumayamanaka 7:a0b9b6e71e41 55 mode = true;
kazumayamanaka 7:a0b9b6e71e41 56 }
kazumayamanaka 7:a0b9b6e71e41 57 if(sb.square()!=0){
kazumayamanaka 7:a0b9b6e71e41 58 mode = false;
kazumayamanaka 7:a0b9b6e71e41 59 }
kazumayamanaka 7:a0b9b6e71e41 60
kazumayamanaka 1:ea880e226e5a 61 theta = bno.euler.yaw * (PI / 180);
kazumayamanaka 1:ea880e226e5a 62
kazumayamanaka 0:a1238c4cd105 63 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 64 theta = -(2 * PI - theta);
kazumayamanaka 0:a1238c4cd105 65 }
kazumayamanaka 0:a1238c4cd105 66
kazumayamanaka 2:e189d55ef292 67 theta = theta - PI / 4;
kazumayamanaka 2:e189d55ef292 68
kazumayamanaka 7:a0b9b6e71e41 69 if(mode == false){
kazumayamanaka 7:a0b9b6e71e41 70 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
kazumayamanaka 0:a1238c4cd105 71
kazumayamanaka 7:a0b9b6e71e41 72 if(DS.cal_input() == true){
kazumayamanaka 7:a0b9b6e71e41 73 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 7:a0b9b6e71e41 74 }
kazumayamanaka 7:a0b9b6e71e41 75 else{
kazumayamanaka 7:a0b9b6e71e41 76 TG.pass_target(DS.obt_X());
kazumayamanaka 7:a0b9b6e71e41 77 }
kazumayamanaka 0:a1238c4cd105 78 }
kazumayamanaka 0:a1238c4cd105 79 else{
kazumayamanaka 7:a0b9b6e71e41 80 automatics();
kazumayamanaka 0:a1238c4cd105 81 }
kazumayamanaka 0:a1238c4cd105 82
kazumayamanaka 0:a1238c4cd105 83 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 84 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 85 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 86 pid_4.pass_val(data_4.get(),TG.obt_target4());
kazumayamanaka 0:a1238c4cd105 87
kazumayamanaka 1:ea880e226e5a 88 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 89 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 90 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 91 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 92
kazumayamanaka 2:e189d55ef292 93 odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
kazumayamanaka 2:e189d55ef292 94 odmetry.pass_angle(theta);
kazumayamanaka 2:e189d55ef292 95 odmetry.print_Pos();
kazumayamanaka 2:e189d55ef292 96 //pc.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());
kazumayamanaka 0:a1238c4cd105 97 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 98 }
kazumayamanaka 7:a0b9b6e71e41 99 automatics();
kazumayamanaka 0:a1238c4cd105 100 }
kazumayamanaka 0:a1238c4cd105 101
kazumayamanaka 7:a0b9b6e71e41 102 double automaticsX(double PosX){
kazumayamanaka 7:a0b9b6e71e41 103 if(PosX > 0){
kazumayamanaka 7:a0b9b6e71e41 104 if(PosX <= odmetry.obt_CurrentPosX())return 0;
kazumayamanaka 7:a0b9b6e71e41 105 else return 100;
kazumayamanaka 7:a0b9b6e71e41 106 }
kazumayamanaka 7:a0b9b6e71e41 107 else{
kazumayamanaka 7:a0b9b6e71e41 108 if(PosX >= odmetry.obt_CurrentPosX())return 0;
kazumayamanaka 7:a0b9b6e71e41 109 else return -100;
kazumayamanaka 7:a0b9b6e71e41 110 }
kazumayamanaka 7:a0b9b6e71e41 111 }
kazumayamanaka 7:a0b9b6e71e41 112 double automaticsY(double PosY){
kazumayamanaka 7:a0b9b6e71e41 113 if(PosY > 0){
kazumayamanaka 7:a0b9b6e71e41 114 if(PosY <= odmetry.obt_CurrentPosY())return 0;
kazumayamanaka 7:a0b9b6e71e41 115 else return -100;
kazumayamanaka 7:a0b9b6e71e41 116 }
kazumayamanaka 7:a0b9b6e71e41 117 else{
kazumayamanaka 7:a0b9b6e71e41 118 if(PosY >= odmetry.obt_CurrentPosY())return 0;
kazumayamanaka 7:a0b9b6e71e41 119 else return 100;
kazumayamanaka 7:a0b9b6e71e41 120 }
kazumayamanaka 7:a0b9b6e71e41 121 }
kazumayamanaka 7:a0b9b6e71e41 122 double automaticsTHETA(double THETA){
kazumayamanaka 7:a0b9b6e71e41 123 if(THETA > 0){
kazumayamanaka 7:a0b9b6e71e41 124 if(THETA <= odmetry.obt_CurrentTheta())return 0;
kazumayamanaka 7:a0b9b6e71e41 125 else return -70;
kazumayamanaka 7:a0b9b6e71e41 126 }
kazumayamanaka 7:a0b9b6e71e41 127 else{
kazumayamanaka 7:a0b9b6e71e41 128 if(THETA >= odmetry.obt_CurrentTheta())return 0;
kazumayamanaka 7:a0b9b6e71e41 129 else return 70;
kazumayamanaka 7:a0b9b6e71e41 130 }
kazumayamanaka 7:a0b9b6e71e41 131 }
kazumayamanaka 7:a0b9b6e71e41 132 void automatics(){
kazumayamanaka 7:a0b9b6e71e41 133 if(automaticsY(100) != 0 || automaticsX(500) !=0){
kazumayamanaka 7:a0b9b6e71e41 134 TG.pass_val(automaticsY(100),automaticsX(500),theta);
kazumayamanaka 7:a0b9b6e71e41 135 }
kazumayamanaka 7:a0b9b6e71e41 136 else{
kazumayamanaka 7:a0b9b6e71e41 137 TG.pass_target(automaticsTHETA(PI / 4));
kazumayamanaka 7:a0b9b6e71e41 138 }
kazumayamanaka 7:a0b9b6e71e41 139 }
kazumayamanaka 7:a0b9b6e71e41 140
kazumayamanaka 7:a0b9b6e71e41 141
kazumayamanaka 0:a1238c4cd105 142 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 143
kazumayamanaka 0:a1238c4cd105 144
kazumayamanaka 0:a1238c4cd105 145