File content as of revision 74:98bad67fde39:

//index_include
#include "mbed.h"
#include "math.h"
#include "bit_test.h"
#include "rs422_put.h"
#include "sbdbt.h"
#include "mecanum.h"
#include "bno055_lib.h"
#include "bno055_use.h"
#include "pid.h"
#include "limit.h"
#include "accelerator.h"
#include "pid_encoder.h"
#include "cyclic_var.h"
#include "cyclic_io.h"
#include "cylinder.h"
#include "event_var.h"

//index_define
#define SBDBT_TX    p13
#define SBDBT_RX    p14
#define SBDBT_STO p12
#define RS422_TX    p28
#define RS422_RX    p27
#define CONTROL_INDICATOR p11

#define SHOULDER_LEFT_HIGH p1
#define SHOULDER_LEFT_LOW p1
#define SHOULDER_RIGHT_HIGH p2
#define SHOULDER_RIGHT_LOW p2

#define GROUND_ENCORDER_X_A p3
#define GROUND_ENCORDER_X_B p4
#define GROUND_ENCORDER_Y_A p5
#define GROUND_ENCORDER_Y_B p6

#define GUN_ENCORDER_A p7
#define GUN_ENCORDER_B p8

#define nucleo_num  4
#define n0_id       0
#define n1_id       1
#define n2_id       2
#define n3_id       3

//index_namespace_const_variable
namespace BAUD {
    const int PC    = 460800;
    const int SBDBT = 115200;
    const int RS422 = 115200;
}

namespace CYCLE {
    const float OUTPUT  = 0.015;
}

namespace MECANUM {
    const float ACCELERATION = 15;
}

namespace POWER {
    const float MECANUM = 1.0;
    const float SWORD   = 1.0;
    const float SHOT = 1.0;
    const float RELOAD = 1.0;
}

//index_namespace_variable
namespace yaw{
    float now;
    float target;
}

//index_Function
void    setup();
void    output();
void    motorCalculation();

//index_setupPin
DigitalOut  led1(LED1);
DigitalOut  led2(LED2);
DigitalOut  led3(LED3);
DigitalOut  led4(LED4);
Serial      pc(USBTX,USBRX);
Rs422       rs422(RS422_TX, RS422_RX);
Sbdbt       sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
Ticker      outputTimer;
Mecanum     mecanum;
Bno055      bno055;
PositionPid yawPid;
Accelerator v1;
Accelerator v2;
Accelerator v3;
Accelerator v4;
CyclicVar   swordSwingCyclic;
DigitalOut  controlIndigator(CONTROL_INDICATOR);


int main()
{
    setup();
    while(1) {
        //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg());
        //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
    }
}

void setup()
{
    wait(1.0);
    bno055.begin();
    wait(1.0);
    bno055.firstRead();
    pc.baud(BAUD::PC);
    sbdbt.begin(BAUD::SBDBT);
    rs422.begin(BAUD::RS422);
    outputTimer.attach(&output, CYCLE::OUTPUT);
    mecanum.setupdeg(bno055.getYawRad());       //基盤が前後逆の場合+180
    v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
    v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
}

//index_mecanum
void motorCalculation(){
    static float out_right_x;
    yaw::now = bno055.getYawRad();
    yaw::target = yaw::now;
    yawPid.calculate(yaw::target, yaw::now); 
    
    //後進時に方向が逆転するため
    if(sbdbt.right_y < 0.0){
        out_right_x = -1.0*sbdbt.right_x;
    }else{
        out_right_x = sbdbt.right_x;
    }
    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
//    pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY());
}


//index_output
void output()
{
    motorCalculation();
    controlIndigator = sbdbt.get_pairingState();
    //led1 = sbdbt.get_pairingState();
    if(sbdbt.left)bno055.yaw_origin();
    
    static int counter;
    int id[nucleo_num] = {n0_id, n1_id, n2_id, n3_id};

    //sbdbtがpairingしている場合のみ動作
    if(sbdbt.get_pairingState()) {
        switch (counter) {
            case 0:
                rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)));
                counter++;
                break;
            case 1:
                //.duty(<cal>*<powerControle>+(<motorBoost>*0.5))
                rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)));
                counter ++;
                break;
            case 2:
                rs422.put(id[counter], limitf(0.0*POWER::SHOT,1.0,-1.0), limitf(0.0*POWER::RELOAD,1.0,-1.0));
                counter ++;
                break;
            case 3:
                rs422.put(id[counter], limitf(0.0*POWER::SWORD,1.0,-1.0), limitf(0.0,1.0,-1.0));
                counter ++;
                break;
            default:
                break;
        }
    }else{
        switch (counter) {
            case 0:
                rs422.put(id[counter],0.0,0.0);
                counter++;
                break;
            case 1:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            case 2:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            case 3:
                rs422.put(id[counter],0.0,0.0);
                counter ++;
                break;
            default:
                break;
        }
    }
}