自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
main.cpp
00001 #include "mbed.h" 00002 #include "rotary_inc.hpp" 00003 #include "PIDco.hpp" 00004 #include "TARGETco.hpp" 00005 #include "DUALSHOCKco.hpp" 00006 #include "Odmetry.hpp" 00007 #include "sbdbt.hpp" 00008 #include "BNO055.h" 00009 #include "AUTOmatics.hpp" 00010 #define SDA D3 00011 #define SCL D6 00012 #define PI 3.1415926535897 00013 #define NORMALMODE true 00014 #define ROUNDMODE false 00015 00016 Serial pc(USBTX,USBRX); 00017 00018 RotaryInc data_1(PA_14,PA_15,0); 00019 RotaryInc data_2(PA_12,PC_5,0); 00020 RotaryInc data_3(PC_0,PC_1,0); 00021 RotaryInc data_4(PC_2,PC_3,0); 00022 00023 RotaryInc MW_1(PA_13,PC_4,0); 00024 RotaryInc MW_2(PC_10,PC_11,0); 00025 RotaryInc MW_3(PA_7,PA_6,0); 00026 RotaryInc MW_4(PA_8,PA_9,0); 00027 00028 PIDco pid_1; 00029 PIDco pid_2; 00030 PIDco pid_3; 00031 PIDco pid_4; 00032 00033 TARGETco TG; 00034 BNO055 bno(SDA,SCL); 00035 DUALSHOCKco DS; 00036 Odmetry odmetry; 00037 sbdbt sb(PA_0,PA_1); 00038 00039 Timer Time; 00040 bool mode=false; 00041 double timer; 00042 double theta; 00043 double automaticsX(double PosX); 00044 double automaticsY(double PosY); 00045 double automaticsTHETA(double THETA); 00046 void automatics(); 00047 void Testautomatics(); 00048 void diff(double PosX,double PosY,double PosT); 00049 double diffX = 0,diffY = 0,diffT = 0; 00050 double targetX=500,targetY=100,targetT=PI / 4; 00051 bool set = true; 00052 00053 int main(){ 00054 Time.start(); 00055 bno.reset(); 00056 00057 while(1){ 00058 timer = Time.read_us(); 00059 bno.setmode(OPERATION_MODE_IMUPLUS); 00060 bno.get_angles(); 00061 00062 if(sb.circle()!=0){ 00063 mode = true; 00064 } 00065 if(sb.square()!=0){ 00066 mode = false; 00067 } 00068 00069 theta = bno.euler.yaw * (PI / 180); 00070 00071 if(theta > PI){ 00072 theta = -(2 * PI - theta); 00073 } 00074 00075 theta = theta - PI / 4; 00076 00077 if(mode == false){ 00078 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); 00079 00080 if(DS.cal_input() == true){ 00081 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); 00082 } 00083 else{ 00084 TG.pass_target(DS.obt_X()); 00085 } 00086 } 00087 else{ 00088 automatics(); 00089 pc.printf("%d ",set); 00090 } 00091 00092 pid_1.pass_val(data_1.get(),TG.obt_target1()); 00093 pid_2.pass_val(data_2.get(),TG.obt_target2()); 00094 pid_3.pass_val(data_3.get(),TG.obt_target3()); 00095 pid_4.pass_val(data_4.get(),TG.obt_target4()); 00096 00097 pid_1.wheel_ctl(PC_9,PC_8); 00098 pid_2.wheel_ctl(PB_14,PB_13); 00099 pid_3.wheel_ctl(PB_5,PB_4); 00100 pid_4.wheel_ctl(PB_7,PB_6); 00101 00102 odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get()); 00103 odmetry.pass_angle(theta); 00104 //odmetry.print_Pos(); 00105 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()); 00106 while(Time.read_us() - timer <= 50 * 1000); 00107 } 00108 } 00109 00110 00111 double automaticsX(){ 00112 if(targetX > 0){ 00113 return diffX * 500 / targetX; 00114 } 00115 else{ 00116 return -diffX * 500 / targetX; 00117 } 00118 } 00119 double automaticsY(){ 00120 if(targetY > 0){ 00121 return -diffY * 500 / targetY; 00122 } 00123 else{ 00124 return diffY * 500 / targetY; 00125 } 00126 } 00127 double automaticsTHETA(){ 00128 if(targetT > 0)return -diffT * 500 / targetT; 00129 else return diffT * 500 / targetT; 00130 } 00131 void automatics(){ 00132 diff(targetX,targetY,targetT); 00133 00134 if((diffX > 1)||(diffY > 1)){ 00135 TG.pass_val(automaticsY(),automaticsX(),theta); 00136 set = true; 00137 } 00138 else set = false; 00139 00140 if(set == false)TG.pass_target(automaticsTHETA()); 00141 00142 printf(" %lf %lf ",diffX,diffY); 00143 } 00144 00145 void diff(double PosX,double PosY,double PosT){ 00146 diffX = PosX - odmetry.obt_CurrentPosX(); 00147 diffY = PosY - odmetry.obt_CurrentPosY(); 00148 diffT = PosT - odmetry.obt_CurrentTheta(); 00149 } 00150 00151 00152 00153 //BNOのピン(PB_3,PB_10) 00154 00155 00156
Generated on Thu Jul 14 2022 19:42:28 by 1.7.2