File content as of revision 0:4143c439ad0e:

// 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);

// motor number
int m_num = 9;

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

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

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

int front_linear_pot;
int t1=0,t2=0,t3=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, 8));
    //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
        if (rid == 6){                                  //id=6からはyaw角度ではなくリニアPOTを受信
            //int front_linear_pot = ((msg.data[6] << 8) + msg.data[7])/100;   // linear_pot data
            //data3 = front_linear_pot;
            }
        pc.printf("%d,%d,%d,%d\r\n", rid, data1, data2, data3);
    }
}

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



int main() {
    can.frequency(50000);
    rx.attach(&RXdata, 0.05);  // 50 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'){ 
                         //if (buf[i-4]-0x30==4){                      //id=4には前部リニアPOTの値を送信
                         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') {
                        /*if (buf[i-4]-0x30==4){                      //id=4には前部リニアPOTの値を送信
                            t1 = front_linear_pot/100;              //100の位
                            t2 = (front_linear_pot-100*t1)/10;      //10の位
                            t3 = front_linear_pot-100*t1-10*t2;     //1の位
                            buf[i-3] = (char)(t1);
                            buf[i-2] = (char)(t2);
                            buf[i-1] = (char)(t3);
                            send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
                            }else{*/
                                send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
                                //}
                                //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;
                        //m_num = ((int)buf[i-3]-0x30)*100+((int)buf[i-2]-0x30)*10+(int)buf[i-1]-0x30;    // Convert from String to Int;
                        id = ((int)buf[i-3]-0x30)*100+((int)buf[i-2]-0x30)*10+(int)buf[i-1]-0x30;    // Convert from String to Int;
                        flag = 1;
                    
                    // ***** Receive mode stop (continuous data) *****
                    // send(ID, mode(1), dir(0), NC, NC, NC)
                    } else if (buf[i-5] == 'F') {
                        id = 0;
                        //m_num = 0;
                        flag = 0;
                    }
                    i = 1;
                    break;
                }
            }
            led1=!led1;
            led2=!led2;
            
            //pc.attach(NULL);//割り込み解除（試験的に導入，2021.2.7, oka）
             
            //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;
        }
    }
}