File content as of revision 11:264f992664b0:

#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[] = {-26.2,26.2};

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;
    }
    
    if(r1 == 1){
        wheel_limit = 0;
    }else{
        wheel_limit = 1;
    }

    pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,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);

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

            get_position(pltheta);
            DS.pass_val(x_component,y_component,r2_num,l2_num);
            //printf("%d\n",y_component);

            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 {
                if(auto_move == 1) {
                    if(position_arrival == false) {
                        x_pos.pid_ctl(x_position,x_pos_target[redZone],13,0.007,0.0005);
                        y_pos.pid_ctl(y_position,y_pos_target[redZone],13,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(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],5,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 <= 0.1 && angle_target[redZone] - robot_angle >= -0.1) {
                                angle_arrival = true;
                                TG.pass_val(64,64,theta);
                                rotate();
                                TG.pass_target(0);
                                rotate();
                                //printf("arrival_angle\n");
                            }
                        }
                    }
                }
            }
            //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);
            //printf("%lf\n",bno.euler.yaw);
            /*if(auto_mode == false) {
                printf("manual\n");
            } else if(auto_mode == true) {
                printf("auto\n");
            }*/
            //printf("%lf\n",pid_angle.output);
            //printf("%lf\n",bno.euler.yaw);

            while(Time.read_ms() - timer <= 50);
        } else {
            TG.pass_val(0,0,theta);
            rotate();
            TG.pass_target(0);
            rotate();
        }
    }
}

//BNOのピン(PB_3,PB_10)