自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
main.cpp
- Committer:
- kazumayamanaka
- Date:
- 2022-03-05
- Revision:
- 8:f2407caf81ba
- Parent:
- 7:a0b9b6e71e41
File content as of revision 8:f2407caf81ba:
#include "mbed.h" #include "rotary_inc.hpp" #include "PIDco.hpp" #include "TARGETco.hpp" #include "DUALSHOCKco.hpp" #include "Odmetry.hpp" #include "sbdbt.hpp" #include "BNO055.h" #include "AUTOmatics.hpp" #define SDA D3 #define SCL D6 #define PI 3.1415926535897 #define NORMALMODE true #define ROUNDMODE false Serial pc(USBTX,USBRX); RotaryInc data_1(PA_14,PA_15,0); RotaryInc data_2(PA_12,PC_5,0); RotaryInc data_3(PC_0,PC_1,0); RotaryInc data_4(PC_2,PC_3,0); RotaryInc MW_1(PA_13,PC_4,0); RotaryInc MW_2(PC_10,PC_11,0); RotaryInc MW_3(PA_7,PA_6,0); RotaryInc MW_4(PA_8,PA_9,0); PIDco pid_1; PIDco pid_2; PIDco pid_3; PIDco pid_4; TARGETco TG; BNO055 bno(SDA,SCL); DUALSHOCKco DS; Odmetry odmetry; sbdbt sb(PA_0,PA_1); Timer Time; bool mode=false; double timer; double theta; double automaticsX(double PosX); double automaticsY(double PosY); double automaticsTHETA(double THETA); void automatics(); void Testautomatics(); void diff(double PosX,double PosY,double PosT); double diffX = 0,diffY = 0,diffT = 0; double targetX=500,targetY=100,targetT=PI / 4; bool set = true; int main(){ Time.start(); bno.reset(); while(1){ timer = Time.read_us(); bno.setmode(OPERATION_MODE_IMUPLUS); bno.get_angles(); if(sb.circle()!=0){ mode = true; } if(sb.square()!=0){ mode = false; } theta = bno.euler.yaw * (PI / 180); if(theta > PI){ theta = -(2 * PI - theta); } theta = theta - PI / 4; if(mode == false){ DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); if(DS.cal_input() == true){ TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); } else{ TG.pass_target(DS.obt_X()); } } else{ automatics(); pc.printf("%d ",set); } pid_1.pass_val(data_1.get(),TG.obt_target1()); pid_2.pass_val(data_2.get(),TG.obt_target2()); pid_3.pass_val(data_3.get(),TG.obt_target3()); pid_4.pass_val(data_4.get(),TG.obt_target4()); pid_1.wheel_ctl(PC_9,PC_8); pid_2.wheel_ctl(PB_14,PB_13); pid_3.wheel_ctl(PB_5,PB_4); pid_4.wheel_ctl(PB_7,PB_6); odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get()); odmetry.pass_angle(theta); //odmetry.print_Pos(); 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()); while(Time.read_us() - timer <= 50 * 1000); } } double automaticsX(){ if(targetX > 0){ return diffX * 500 / targetX; } else{ return -diffX * 500 / targetX; } } double automaticsY(){ if(targetY > 0){ return -diffY * 500 / targetY; } else{ return diffY * 500 / targetY; } } double automaticsTHETA(){ if(targetT > 0)return -diffT * 500 / targetT; else return diffT * 500 / targetT; } void automatics(){ diff(targetX,targetY,targetT); if((diffX > 1)||(diffY > 1)){ TG.pass_val(automaticsY(),automaticsX(),theta); set = true; } else set = false; if(set == false)TG.pass_target(automaticsTHETA()); printf(" %lf %lf ",diffX,diffY); } void diff(double PosX,double PosY,double PosT){ diffX = PosX - odmetry.obt_CurrentPosX(); diffY = PosY - odmetry.obt_CurrentPosY(); diffT = PosT - odmetry.obt_CurrentTheta(); } //BNOのピン(PB_3,PB_10)