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)