File content as of revision 7:a0375e6dc8ca:

#include "mbed.h"
#include "rotary_inc.hpp"
#include "PIDco.hpp"
#include "TARGETco.hpp"
#include "DUALSHOCKco.hpp"
#include "sbdbt.hpp"
#include "BNO055.h"
#include "scrp_slave.hpp"
#include "function.hpp"
#include "odometry.hpp"

#define SDA D3
#define SCL D6
#define PI 3.1415926535897

ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);

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);

PIDco pid_1;
PIDco pid_2;
PIDco pid_3;
PIDco pid_4;

TARGETco TG;
BNO055 bno(SDA,SCL);
DUALSHOCKco DS;

//sbdbt sb(PA_0,PA_1);

DigitalOut led1(PA_10);
DigitalOut led2(PB_15);
DigitalOut led3(PB_2);
DigitalOut led4(PC_6);

Timer Time;
double timer;
double theta;
double X,Y;
double auto_x_component;
double auto_y_component;

//for odometry
double theta2;
double pltheta;

double x_pos_target[] = {15.0};
double y_pos_target[] = {0};
double angle_target;

double max_mini(double a){
    if(a > 450){
        a = 450;
    }else if(a < -450){
        a = -450;
    }else if(a == 0){
        a = 0;
    }
    return a;
} 

int main(){
    bno.reset();
    Time.start();
    TG.pass_val(0,0,0);
    scrp.addCMD(1,getLstick_x);
    scrp.addCMD(2,getLstick_y);
    scrp.addCMD(3,getL2);
    scrp.addCMD(4,getR2);
    scrp.addCMD(5,change_mode);

    
    while(1){
        timer = Time.read_us();
        bno.setmode(OPERATION_MODE_IMUPLUS);
        bno.get_angles();
        
        theta = bno.euler.yaw * (PI / 180);
        theta2 = (90 - bno.euler.yaw) * (PI /180);
        
        if(theta > PI){
            theta = -(2 * PI - theta);

        }else{
        }
        theta = theta - (PI / 4);
        
        if(theta2 > PI){
            pltheta = theta2 - 2 * PI;
        }else{
            pltheta = theta2;    
        }
        
        get_position(pltheta);
        DS.pass_val(x_component,y_component,r2_num,l2_num);
        
        if(auto_mode == false){
            if(DS.cal_input() == true){
                TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
            }else{
                TG.pass_target(DS.obt_X());
            }
        }else{
            auto_x_component = max_mini((x_pos_target[0] - x_position) * 150);
            auto_y_component = max_mini((y_pos_target[0] - y_position) * 150);
            TG.pass_val(auto_y_component,auto_x_component,theta);
        }
        
        pid_1.pass_val(data_1.get(),TG.obt_target1(),0.00007,0.000001,0.00000014);
        pid_2.pass_val(data_2.get(),TG.obt_target2(),0.00007,0.000001,0.00000014);
        pid_3.pass_val(data_3.get(),TG.obt_target3(),0.00007,0.000001,0.00000014);
        pid_4.pass_val(data_4.get(),TG.obt_target4(),0.00007,0.000001,0.00000014);
        
        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);
        
        if(TG.obt_target1() != 0){
            led1 = 1;
        }else{
            led1 = 0;
        }
        if(TG.obt_target2() != 0){
            led2 = 1;
        }else{
            led2 = 0;
        }
        if(TG.obt_target3() != 0){
            led3 = 1;
        }else{
            led3 = 0;
        }
        if(TG.obt_target4() != 0){
            led4 = 1;
        }else{
            led4 = 0;
        }
        
        //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());
        printf("%lf,%lf\n",x_position,y_position);
        /*if(auto_mode == false){
            printf("manual\n");
        }else if(auto_mode == true){
            printf("auto\n");
        }*/
          
        while(Time.read_us() - timer <= 50 * 1000);
        }
}

//BNOのピン(PB_3,PB_10)