File content as of revision 0:35bb9af90b04:

#include "mbed.h"
#include "Mycan.h"

Mycan can(p30, p29, 100000);   //// CAN (PinName rd, PinName td)

DigitalOut LED[4] = 
{
    DigitalOut(LED1),
    DigitalOut(LED2),
    DigitalOut(LED3),
    DigitalOut(LED4),
};

DigitalOut led_R(p14);
DigitalOut led_B(p18);

DigitalIn sw[11] = 
{
    DigitalIn(p5),
    DigitalIn(p6),
    DigitalIn(p8),
    DigitalIn(p10),
    DigitalIn(p12),
    DigitalIn(p7),
    DigitalIn(p9),
    DigitalIn(p11),
    DigitalIn(p13),
    DigitalIn(p16),
    DigitalIn(p15)
};

Ticker ticker;
Serial pc(USBTX, USBRX);

void canFnc();
void readLineData();
bool T_sw[4], B_sw[4], S_sw, Z_sw, M_sw;
short int send_val[2], line_data[2];

int main() 
{
    for(int i=0; i<11; i++)
        sw[i].mode(PullUp);
    ticker.attach(&canFnc, 0.0065);
    while(1) 
    {
        S_sw = sw[0];
        pc.printf("%d\t", S_sw);
        for(int i=0; i<4; i++) {
            B_sw[i] = sw[i+1];
            pc.printf("%d\t", B_sw[i]);
        }
        for(int i=0; i<4; i++) {
            T_sw[i] = sw[i+5];
            pc.printf("%d\t", T_sw[i]);
        }
        Z_sw = sw[9];
        if(Z_sw) {
            led_R = 1;
            led_B = 0;
        } else {
            led_R = 0;
            led_B = 1;
        }
        M_sw = sw[10];
        
        pc.printf("%d\t", Z_sw);
        pc.printf("%d\t", M_sw);
        pc.printf("%d\t", send_val[0]);
        pc.printf("%d\t", send_val[1]);
        pc.printf("%d\t", line_data[0]);
        pc.printf("%d\t", line_data[1]);
        pc.printf("\n");
    }
}

void canFnc() 
{
    send_val[0] = send_val[1] = 0;
    if(S_sw) 
        send_val[0] += 64;
    if(B_sw[0])
        send_val[0] += 32;
    if(B_sw[1])
        send_val[0] += 16;
    if(B_sw[2])
        send_val[0] += 8;
    if(B_sw[3])
        send_val[0] += 4;
    if(!T_sw[0])
        send_val[0] += 2;
    if(!T_sw[1])
        send_val[0] += 1;
    if(!T_sw[2])
        send_val[1] += 8;
    if(!T_sw[3])
        send_val[1] += 4;
    if(Z_sw)
        send_val[1] += 2;
    if(M_sw)
        send_val[1] += 1;
    
    readLineData();
    
    can.setI(2, 0, (short int)(send_val[0]));
    can.setI(2, 1, (short int)(send_val[1]));
    can.setI(2, 2, (short int)(line_data[0]));
    can.setI(2, 3, (short int)(line_data[1]));
    LED[0] = can.send();
}

void readLineData() 
{
    int read_line_data[4];
    
    for(int i = 0; i < 4; i++) {
        can.readI();
        read_line_data[i] = can.get(i+6, 0);
    }
    
    int residue_data, true_num[4], true_all[4];
    
    for(int i = 0; i < 4; i++) 
    {
        residue_data = read_line_data[i];
        true_all[i] = true_num[i] = 0;
        
        if(residue_data < 0) {
            if(residue_data == -200) 
                residue_data = 0;
            else residue_data *= -1;
            
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 7 + 8;
            else true_all[i] += 7;
        }
        
        if(residue_data >= 64) {
            residue_data -= 64;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 6 + 8;
            else true_all[i] += 6;
        }
        
        if(residue_data >= 32) {
            residue_data -= 32;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 5 + 8;
            else true_all[i] += 5;
        }
        
        if(residue_data >= 16) {
            residue_data -= 16;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 4 + 8;
            else true_all[i] += 4;
        }
        
        if(residue_data >= 8) {
            residue_data -= 8;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 3 + 8;
            else true_all[i] += 3;
        }
        
        if(residue_data >= 4) {
            residue_data -= 4;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 2 + 8;
            else true_all[i] += 2;
        }
        
        if(residue_data >= 2) {
            residue_data -= 2;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 1 + 8;
            else true_all[i] += 1;
        }
        
        if(residue_data >= 1) {
            residue_data -= 1;
            true_num[i] += 1;
            if(i == 1 || i == 3)
                true_all[i] += 8;
        }
    }
    
    if(true_all[0] + true_all[1] != 0 || true_num[0] + true_num[1] != 0)
        line_data[0] = (true_all[0] + true_all[1]) / (true_num[0] + true_num[1]);
    else line_data[0] = 0;
    
    if(true_all[2] + true_all[3] != 0 || true_num[2] + true_num[3] != 0)
        line_data[1] = (true_all[2] + true_all[3]) / (true_num[2] + true_num[3]);
    else line_data[1] = 0;
}