kazuma yamanaka / 2022_Ateam_MOTORprogram

自己位置推定機能を追加

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@8:f2407caf81ba, 2022-03-05 (annotated)

Committer:
kazumayamanaka
Date:
Sat Mar 05 01:00:42 2022 +0000
Revision:
8:f2407caf81ba
Parent:
7:a0b9b6e71e41 
program of motor

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 8:f2407caf81ba 9 #include "AUTOmatics.hpp"
kazumayamanaka 0:a1238c4cd105 10 #define SDA D3
kazumayamanaka 0:a1238c4cd105 11 #define SCL D6
kazumayamanaka 0:a1238c4cd105 12 #define PI 3.1415926535897
kazumayamanaka 8:f2407caf81ba 13 #define NORMALMODE true
kazumayamanaka 8:f2407caf81ba 14 #define ROUNDMODE false
kazumayamanaka 0:a1238c4cd105 15
kazumayamanaka 0:a1238c4cd105 16 Serial pc(USBTX,USBRX);
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 2:e189d55ef292 23 RotaryInc MW_1(PA_13,PC_4,0);
kazumayamanaka 2:e189d55ef292 24 RotaryInc MW_2(PC_10,PC_11,0);
kazumayamanaka 2:e189d55ef292 25 RotaryInc MW_3(PA_7,PA_6,0);
kazumayamanaka 2:e189d55ef292 26 RotaryInc MW_4(PA_8,PA_9,0);
kazumayamanaka 2:e189d55ef292 27
kazumayamanaka 0:a1238c4cd105 28 PIDco pid_1;
kazumayamanaka 0:a1238c4cd105 29 PIDco pid_2;
kazumayamanaka 0:a1238c4cd105 30 PIDco pid_3;
kazumayamanaka 0:a1238c4cd105 31 PIDco pid_4;
kazumayamanaka 0:a1238c4cd105 32
kazumayamanaka 0:a1238c4cd105 33 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 34 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 35 DUALSHOCKco DS;
kazumayamanaka 2:e189d55ef292 36 Odmetry odmetry;
kazumayamanaka 0:a1238c4cd105 37 sbdbt sb(PA_0,PA_1);
kazumayamanaka 0:a1238c4cd105 38
kazumayamanaka 0:a1238c4cd105 39 Timer Time;
kazumayamanaka 7:a0b9b6e71e41 40 bool mode=false;
kazumayamanaka 0:a1238c4cd105 41 double timer;
kazumayamanaka 0:a1238c4cd105 42 double theta;
kazumayamanaka 7:a0b9b6e71e41 43 double automaticsX(double PosX);
kazumayamanaka 7:a0b9b6e71e41 44 double automaticsY(double PosY);
kazumayamanaka 7:a0b9b6e71e41 45 double automaticsTHETA(double THETA);
kazumayamanaka 7:a0b9b6e71e41 46 void automatics();
kazumayamanaka 8:f2407caf81ba 47 void Testautomatics();
kazumayamanaka 8:f2407caf81ba 48 void diff(double PosX,double PosY,double PosT);
kazumayamanaka 8:f2407caf81ba 49 double diffX = 0,diffY = 0,diffT = 0;
kazumayamanaka 8:f2407caf81ba 50 double targetX=500,targetY=100,targetT=PI / 4;
kazumayamanaka 8:f2407caf81ba 51 bool set = true;
kazumayamanaka 0:a1238c4cd105 52
kazumayamanaka 0:a1238c4cd105 53 int main(){
kazumayamanaka 0:a1238c4cd105 54 Time.start();
kazumayamanaka 7:a0b9b6e71e41 55 bno.reset();
kazumayamanaka 2:e189d55ef292 56
kazumayamanaka 0:a1238c4cd105 57 while(1){
kazumayamanaka 0:a1238c4cd105 58 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 59 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 60 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 61
kazumayamanaka 7:a0b9b6e71e41 62 if(sb.circle()!=0){
kazumayamanaka 7:a0b9b6e71e41 63 mode = true;
kazumayamanaka 7:a0b9b6e71e41 64 }
kazumayamanaka 7:a0b9b6e71e41 65 if(sb.square()!=0){
kazumayamanaka 7:a0b9b6e71e41 66 mode = false;
kazumayamanaka 7:a0b9b6e71e41 67 }
kazumayamanaka 7:a0b9b6e71e41 68
kazumayamanaka 1:ea880e226e5a 69 theta = bno.euler.yaw * (PI / 180);
kazumayamanaka 1:ea880e226e5a 70
kazumayamanaka 0:a1238c4cd105 71 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 72 theta = -(2 * PI - theta);
kazumayamanaka 0:a1238c4cd105 73 }
kazumayamanaka 0:a1238c4cd105 74
kazumayamanaka 2:e189d55ef292 75 theta = theta - PI / 4;
kazumayamanaka 2:e189d55ef292 76
kazumayamanaka 7:a0b9b6e71e41 77 if(mode == false){
kazumayamanaka 7:a0b9b6e71e41 78 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
kazumayamanaka 0:a1238c4cd105 79
kazumayamanaka 7:a0b9b6e71e41 80 if(DS.cal_input() == true){
kazumayamanaka 7:a0b9b6e71e41 81 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 7:a0b9b6e71e41 82 }
kazumayamanaka 7:a0b9b6e71e41 83 else{
kazumayamanaka 7:a0b9b6e71e41 84 TG.pass_target(DS.obt_X());
kazumayamanaka 7:a0b9b6e71e41 85 }
kazumayamanaka 0:a1238c4cd105 86 }
kazumayamanaka 0:a1238c4cd105 87 else{
kazumayamanaka 7:a0b9b6e71e41 88 automatics();
kazumayamanaka 8:f2407caf81ba 89 pc.printf("%d ",set);
kazumayamanaka 0:a1238c4cd105 90 }
kazumayamanaka 0:a1238c4cd105 91
kazumayamanaka 0:a1238c4cd105 92 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 93 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 94 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 95 pid_4.pass_val(data_4.get(),TG.obt_target4());
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
kazumayamanaka 2:e189d55ef292 102 odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
kazumayamanaka 2:e189d55ef292 103 odmetry.pass_angle(theta);
kazumayamanaka 8:f2407caf81ba 104 //odmetry.print_Pos();
kazumayamanaka 8:f2407caf81ba 105 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 106 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 107 }
kazumayamanaka 0:a1238c4cd105 108 }
kazumayamanaka 0:a1238c4cd105 109
kazumayamanaka 8:f2407caf81ba 110
kazumayamanaka 8:f2407caf81ba 111 double automaticsX(){
kazumayamanaka 8:f2407caf81ba 112 if(targetX > 0){
kazumayamanaka 8:f2407caf81ba 113 return diffX * 500 / targetX;
kazumayamanaka 7:a0b9b6e71e41 114 }
kazumayamanaka 7:a0b9b6e71e41 115 else{
kazumayamanaka 8:f2407caf81ba 116 return -diffX * 500 / targetX;
kazumayamanaka 7:a0b9b6e71e41 117 }
kazumayamanaka 7:a0b9b6e71e41 118 }
kazumayamanaka 8:f2407caf81ba 119 double automaticsY(){
kazumayamanaka 8:f2407caf81ba 120 if(targetY > 0){
kazumayamanaka 8:f2407caf81ba 121 return -diffY * 500 / targetY;
kazumayamanaka 7:a0b9b6e71e41 122 }
kazumayamanaka 7:a0b9b6e71e41 123 else{
kazumayamanaka 8:f2407caf81ba 124 return diffY * 500 / targetY;
kazumayamanaka 7:a0b9b6e71e41 125 }
kazumayamanaka 7:a0b9b6e71e41 126 }
kazumayamanaka 8:f2407caf81ba 127 double automaticsTHETA(){
kazumayamanaka 8:f2407caf81ba 128 if(targetT > 0)return -diffT * 500 / targetT;
kazumayamanaka 8:f2407caf81ba 129 else return diffT * 500 / targetT;
kazumayamanaka 7:a0b9b6e71e41 130 }
kazumayamanaka 7:a0b9b6e71e41 131 void automatics(){
kazumayamanaka 8:f2407caf81ba 132 diff(targetX,targetY,targetT);
kazumayamanaka 8:f2407caf81ba 133
kazumayamanaka 8:f2407caf81ba 134 if((diffX > 1)||(diffY > 1)){
kazumayamanaka 8:f2407caf81ba 135 TG.pass_val(automaticsY(),automaticsX(),theta);
kazumayamanaka 8:f2407caf81ba 136 set = true;
kazumayamanaka 7:a0b9b6e71e41 137 }
kazumayamanaka 8:f2407caf81ba 138 else set = false;
kazumayamanaka 8:f2407caf81ba 139
kazumayamanaka 8:f2407caf81ba 140 if(set == false)TG.pass_target(automaticsTHETA());
kazumayamanaka 8:f2407caf81ba 141
kazumayamanaka 8:f2407caf81ba 142 printf(" %lf %lf ",diffX,diffY);
kazumayamanaka 7:a0b9b6e71e41 143 }
kazumayamanaka 8:f2407caf81ba 144
kazumayamanaka 8:f2407caf81ba 145 void diff(double PosX,double PosY,double PosT){
kazumayamanaka 8:f2407caf81ba 146 diffX = PosX - odmetry.obt_CurrentPosX();
kazumayamanaka 8:f2407caf81ba 147 diffY = PosY - odmetry.obt_CurrentPosY();
kazumayamanaka 8:f2407caf81ba 148 diffT = PosT - odmetry.obt_CurrentTheta();
kazumayamanaka 8:f2407caf81ba 149 }
kazumayamanaka 8:f2407caf81ba 150
kazumayamanaka 7:a0b9b6e71e41 151
kazumayamanaka 7:a0b9b6e71e41 152
kazumayamanaka 0:a1238c4cd105 153 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 154
kazumayamanaka 0:a1238c4cd105 155
kazumayamanaka 0:a1238c4cd105 156