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