File content as of revision 14:aac2f18f6779:

#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 "encorder.h"
#include "cyclic.h"
#include "cyclic_IO.h"
//#include "QEI.h"

#define pc_baud     460800
#define sbdbt_tx    p13
#define sbdbt_rx    p14
#define sbdbt_baud  115200
#define rs422_tx    p28
#define rs422_rx    p27
#define rs422_baud  115200
#define output_period   0.015
#define nucleo_num  3
#define pi          3.141592
#define n1_id       3
#define n2_id       4
#define n3_id       5
#define n4_id       6
#define n5_id       7
#define yaw_Kp      0.01
#define yaw_Ki      0.01
#define yaw_Kd      0.01
#define acceleration    25
#define pin_cylinder_on     p17
#define pin_cylinder_off    p18
#define pin_interrupt       p23
#define encoder_A       p25
#define encoder_B       p26
#define enc_Kp      0.01
#define enc_Ki      0.01
#define enc_Kd      0.01

DigitalOut led1(LED1);
DigitalIn interrupt(pin_interrupt);
Serial pc(USBTX,USBRX);
RS422 rs422(rs422_tx, rs422_rx);
Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
Ticker output_timer;
Mecanum mecanum;
Bno055 bno055;
Position_pid yaw_pid;
Accel v1;
Accel v2;
Accel v3;
Accel v4;
Cyclic_IO led2(LED2);
Cyclic_IO sylinder_on(pin_cylinder_on);
Cyclic_IO sylinder_off(pin_cylinder_off);
Cyclic cylinder_cyclic;

//追加点
Encoder enc_cylinder(encoder_A,encoder_B);
//QEI wheel(encoder_A, encoder_B, NC, 624);

void setup();
void output();
void put_output();
void cylinder_check();
void boost();
void cylinder_origin();
float yaw, target_yaw;

int main()
{
    setup();
    while(1) {
        //led1 = interrupt;
        //pc.printf("Pulses is: %i\r\n",wheel.getPulses());
    }
}

void setup()
{
    wait(1);
    bno055.begin();
    wait(1);
    bno055.firstRead();
    pc.baud(pc_baud);
    sbdbt.begin(sbdbt_baud);
    rs422.begin(rs422_baud);
    cylinder_origin();
    output_timer.attach(&output, output_period);
    yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
    mecanum.setupdeg(bno055.getYawRad()+180.0);
    v1.setup(acceleration,output_period);
    v2.setup(acceleration,output_period);
    v3.setup(acceleration,output_period);
    v4.setup(acceleration,output_period);

//追加点
    enc_cylinder.setup(1200);
    enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
}

void cylinder_origin(){
    while(interrupt){
        led1 = 1;
        rs422.put(5, -0.8, 0.0);
    }
    led1 = 0;
}

void boost(){
    if(sbdbt.sankaku) {
        mecanum.boost_forward();
    }
    if(sbdbt.batu) {
        mecanum.boost_back();
    }
    if(sbdbt.shikaku) {
        mecanum.boost_left();
    }
    if(sbdbt.maru) {
        mecanum.boost_right();
    }
}

void cylinder_check()
{
    //printf("up\t%d\tstate\t%d\tdown\t%d\tstate\t%d\t\r\n",sbdbt.up,sylinder_on.getState(),sbdbt.down,sylinder_off.getState());
    /*
    if(sylinder_on?){
    sylinder_on.cyclic(sbdbt.up);
    }else if(){
    sylinder_off.cyclic(sbdbt.down); 
    }
    led2.cyclic(sbdbt.left);
    */
    
    cylinder_cyclic.cyclic(sbdbt.up);
    if(cylinder_cyclic.getState()== 1){
        led1 = 1;
    }else{
        led1 = 0;
    }
}

void put_output()
{
    yaw = bno055.getYawRad();
    target_yaw = yaw;
    yaw_pid.cal(target_yaw, yaw, output_period);
    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), 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());
}

void output()
{
    put_output();
    cylinder_check();
    boost();

    static int counter;
    int id[nucleo_num] = {n1_id, n2_id, n3_id};

    switch (counter) {
        case 0:
            rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
            counter++;
            break;
        case 1:
            rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
            counter ++;
            break;
        case 2:
            rs422.put(id[counter], sbdbt.right_y, 0.0);
            counter = 0;
            break;
        default:
            break;
    };
}