File content as of revision 13:369f4abc1f36:

#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,1);

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;

PIDco pid_angle;
PIDco x_pos;
PIDco y_pos;

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

//sbdbt sb(PA_0,PA_1);

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

Timer Time;
long double timer;
double theta;
double X,Y;
double auto_x_component;
double auto_y_component;
bool position_arrival = false;
bool angle_arrival = false;
double regulation;
bool wheel_limit;

//for odometry
double theta2;
double pltheta;

//target
double x_pos_target[] = {81.72,81.72};
double y_pos_target[] = {-90.47,90.47};
double angle_target[] = {-19.0,19.0};

double robot_angle;

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

void rotate()
{
    if(l1 == 1) {
        regulation = 0.3;
    } else {
        regulation = 1.0;
    }

    if(r1 == 1) {
        wheel_limit = 0.0;
    } else {
        wheel_limit = 1.0;
    }

    pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,0.0,0);
    pid_2.pass_val(data_2.get(),TG.obt_target2() * regulation,0.00005,0.0,0);
    pid_3.pass_val(data_3.get(),TG.obt_target3() * regulation,0.00005,0.0,0);
    pid_4.pass_val(data_4.get(),TG.obt_target4() * regulation,0.00005,0.0,0);

    pid_1.wheel_ctl(PC_9,PC_8,wheel_limit);
    pid_2.wheel_ctl(PB_14,PB_13,wheel_limit);
    pid_3.wheel_ctl(PB_5,PB_4,wheel_limit);
    pid_4.wheel_ctl(PB_7,PB_6,wheel_limit);

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

void VectorDrection4(int x,int y,double theta)
{
    x = x - 64;
    y = y - 64;
    //printf("%d %d\n",x,y);
    if(y > -abs(x)) { //上
        DS2.pass_val(64,y + 64,0,0);
    }
    if(y < abs(x)) { //下
        DS2.pass_val(64,y + 64,0,0);
    }
    if(x < -abs(y)) { //左
        DS2.pass_val(x + 64,64,0,0);
    }
    if(x > abs(y)) { //右
        DS2.pass_val(x + 64,64,0,0);
    }
    if(y == 0 && x == 0) {
        DS2.pass_val(64,64,0,0);
    }
    DS2.cal_input();
    //printf("%lf %lf\n",DS2.obt_X(),DS2.obt_Y());
    TG.pass_val(DS2.obt_X(),DS2.obt_Y(),theta);
    rotate();
}

//-------------------------------------

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,getL1);
    scrp.addCMD(6,getCircle);
    scrp.addCMD(7,getZone);
    scrp.addCMD(8,getR1);
    scrp.addCMD(9,getRstick_x);
    scrp.addCMD(10,getRstick_y);
    scrp.addCMD(50,change_mode);
    scrp.addCMD(51,getStop);


    while(1) {
        TG.pass_val(0,0,theta);
        if(stop == 0) {
            timer = Time.read_ms();
            bno.setmode(OPERATION_MODE_IMUPLUS);
            bno.get_angles();
            if(auto_mode == false) {
                if(redZone == true) {
                    theta = (bno.euler.yaw - 90) * (PI / 180);
                    theta2 = (bno.euler.yaw - 180) * (PI /180);
                } else {
                    theta = (bno.euler.yaw + 90) * (PI / 180);
                    theta2 = bno.euler.yaw * (PI /180);
                }
            } else {
                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;
            }

            
            DS.pass_val(x_component,y_component,r2_num,l2_num);

            if(auto_mode == false) {
                DS.cal_input();
                TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
                rotate();
                DS.cal_input();
                TG.pass_target(-1 * DS.obt_rotate());
                rotate();
                VectorDrection4(l_x_component,l_y_component,theta);
            } else {
                get_position(pltheta);
                if(auto_move == 1) {
                    if(position_arrival == false) {
                        x_pos.pid_ctl(x_position,x_pos_target[redZone],20.0,0.007,0.0005);
                        y_pos.pid_ctl(y_position,y_pos_target[redZone],20.0,0.007,0.0005);
                        auto_x_component = max_mini(x_pos.output);
                        auto_y_component = max_mini(y_pos.output);
                        TG.pass_val(auto_y_component,auto_x_component,theta);
                        rotate();
                        //printf("go to target\n");
                        if((x_pos_target[redZone] - x_position) <= 0.5 && (x_pos_target[redZone] - x_position) >= -0.5 && (y_pos_target[redZone] - y_position) <= 0.5 && (y_pos_target[redZone] - y_position) >= -0.5) {
                            position_arrival = true;
                            TG.pass_val(64,64,theta);
                            rotate();
                            printf("arrival_position\n");
                        }
                    } else {
                        if(r1 == 1) {
                            TG.pass_val(0,0,theta);
                            rotate();
                            TG.pass_target(0);
                            rotate();
                        } else {
                            if(angle_arrival == false) {
                                //printf("go to angle_target\n");
                                if(bno.euler.yaw >=  0 && bno.euler.yaw <= 180) {
                                    robot_angle = bno.euler.yaw;
                                } else {
                                    robot_angle = bno.euler.yaw - 360;
                                }
                                pid_angle.pid_ctl(robot_angle,angle_target[redZone],6.0,0.007,0.0001);
                                printf("%lf,%lf\n",pid_angle.output,robot_angle);
                                TG.pass_target(-1 * pid_angle.output);
                                rotate();
                                if(angle_target[redZone] - robot_angle <= 1 && angle_target[redZone] - robot_angle >= -1) {
                                    angle_arrival = true;
                                    TG.pass_val(64,64,theta);
                                    rotate();
                                    TG.pass_target(0);
                                    rotate();
                                    //printf("arrival_angle\n");
                                }
                            }
                        }
                    }
                }
            }

            while(Time.read_ms() - timer <= 50);
            if(Time.read_ms() > 2000000){
                Time.reset();
            }
        } else if(stop == 1) {
            TG.pass_val(0,0,theta);
            rotate();
            TG.pass_target(0);
            rotate();
        } else if(stop == 3) {
            NVIC_SystemReset();
        }
    }
}

//BNOのピン(PB_3,PB_10)