File content as of revision 2:d5b8f8e62923:

#include "mbed.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"

#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.005
#define nucleo_num  3
#define pi          3.141592
#define n1_id       3
#define n2_id       4
#define n3_id       0
#define yaw_Kp      0.07
#define yaw_Ki      0
#define yaw_Kd      0

DigitalOut led(LED1);
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;


void setup();
void output();
void put_output();
float m1, m2, m3, m4;
float a;
float yaw, target_yaw;

int main()
{
    setup();
    while(1) {
        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());
        put_output();
    }
}

void setup()
{
    wait(1);
    bno055.begin();
    wait(1);
    bno055.firstRead();
    pc.baud(pc_baud);
    sbdbt.begin(sbdbt_baud);
    rs422.begin(rs422_baud);
    output_timer.attach(&output, output_period);
    yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
    mecanum.setupdeg(bno055.getYawRad());
}

void put_output(){
    m1 = mecanum.v1();
    m2 = mecanum.v2();
    m3 = mecanum.v3();
    m4 = mecanum.v4();
}

void output()
{
    static int counter;
    int id[nucleo_num] = {n1_id, n2_id, n3_id};

    switch (counter) {
        case 0:
            rs422.put(id[counter],m1 ,m3);
            counter++;
            break;
        case 1:
            rs422.put(id[counter],m2 ,m4);
            counter ++;
            break;
        case 2:
            rs422.put(id[counter], 0.0, 0.0);
            counter = 0;
            break;
        default:
            break;
    };
}