File content as of revision 0:22b26272c47e:

// Nucleo_CAN_master for AIRo-4.1
// Created by Atsushi Kakogawa, 2020.03.20
// Modified by ****** ******, 2020.**.**
// Department of Robotics, Ritsumeikan University, Japan

#include "mbed.h"
#include "CAN.h"
    
DigitalOut led1(LED1);
DigitalOut led2(D8);
DigitalOut led3(D4);
Serial pc(USBTX, USBRX, 115200);    // Serial PC

Ticker rx; // timer for data receiver

//cancom;
CAN can(PA_11, PA_12);

// from a master computer
int i=0;
char buf[255];

// converter
int mode, dir;
char tx_data[8];

// Data-receiver
int id = 0;
int flag = 0;

// CAN communication to slaves
void send(int id, int mode, int dir, char targ3, char targ2, char targ1) {
    int target_ang = ((int)targ3-0x30)*100+((int)targ2-0x30)*10+(int)targ1-0x30;    // Convert from String to Int
    char tx_target_U = (target_ang >> 8) & 0xff;                                    // Split the Int data to upper
    char tx_target_L = target_ang & 0xff;                                           // Split the Int data to lower
    tx_data[0] = id;             // ID
    tx_data[1] = mode;          // mode (0: control, 1: response) 
    tx_data[2] = dir;           // dir (0: positive, 1: negative)
    tx_data[3] = tx_target_U;   // target value upper 8bit 
    tx_data[4] = tx_target_L;   // target value lower 8bit
    tx_data[5] = 0;
    tx_data[6] = 0;
    tx_data[7] = 0;
    can.write(CANMessage(1337, tx_data, 5));
    //pc.printf("Sent(0x): %x %x %x %x %x %x %x %x\r\n", tx_data[0], tx_data[1], tx_data[2], tx_data[3], tx_data[4], tx_data[5], tx_data[6], tx_data[7]);
}

void receive() {
    CANMessage msg;
    // CAN read from slaves and send to a master computer
    if(can.read(msg)) {
        int rid = msg.data[0];
        int data1 = (msg.data[2] << 8) + msg.data[3];   // current
        int data2 = (msg.data[4] << 8) + msg.data[5];   // velocity
        int data3 = (msg.data[6] << 8) + msg.data[7];   // angle
        //pc.printf("%d,%.2f,%.2f,%.2f\r\n", id, data1, data2, data3);
        pc.printf("%d,%d,%d,%d\r\n", rid, data1, data2, data3);
    }
}

// Data-receiver timer
void RXdata() {
    if (flag == 1) {
        for (id=1; id<3; id++) {
            send(id,1,0,0,0,0);                            // Receive command
            receive();
        }
    }
}

int main() {
    
    //pc.printf("0,0\r\n");
    can.frequency(1000000);
    rx.attach(&RXdata, 0.2);  // 200 ms
    
    while(1) {

        // Serial read from a master computer
        if (pc.getc()) {
            for (i=1; i<256; i++) {
                buf[i] = pc.getc();
                if (buf[i] == '\n') {
                    
                    // ***** Angle control mode *****
                    // send(ID, mode(0), dir(0 or 1), 3rd digit, 2rd digit, 1rd digit)
                    if (buf[i-5] == 'A'){ 
                        send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
                        
                    // ***** Receive mode (single data) *****
                    // send(ID, mode(1), dir(0), NC, NC, NC)
                    } else if (buf[i-5] == 'B') {
                        send(buf[i-4]-0x30,1,0,0,0,0);
                        receive();
                        
                    // ***** Speed control mode (positive) *****
                    // send(ID, mode(0), dir(0), 3rd digit, 2rd digit, 1rd digit)
                    } else if (buf[i-5] == 'C') {
                        send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
                        
                    // ***** Speed control mode (negative) *****
                    // send(ID, mode(0), dir(1), 3rd digit, 2rd digit, 1rd digit)
                    } else if (buf[i-5] == 'D') {
                        send(buf[i-4]-0x30,0,1,buf[i-3],buf[i-2],buf[i-1]);
                    
                    // ***** Receive mode start (continuous data) *****
                    // send(ID, mode(1), dir(0), NC, NC, NC)
                    } else if (buf[i-5] == 'E') {
                        id = buf[i-4]-0x30;
                        flag = 1;
                    
                    // ***** Receive mode stop (continuous data) *****
                    // send(ID, mode(1), dir(0), NC, NC, NC)
                    } else if (buf[i-5] == 'F') {
                        id = 0;
                        flag = 0;
                    }
                    i = 1;
                    break;
                }
            }
            led1=!led1;
            led2=!led2;
             
            //wait (0.001); // Eriminate this "wait" when linux
        }
        
        // CAN error check
        if(can.rderror()){
            pc.printf("rx error,%d\r\n",can.rderror());
            can.reset();
            led3=1;
            wait (0.1);
        } else {
            led3=0;
        }
        if(can.tderror()){
            pc.printf("tx error,%d\r\n",can.tderror());
            can.reset();
            led3=1;
            wait (0.1);
        } else {
            led3=0;
        }
    }
}