File content as of revision 4:56d1f3a81b30:

#include "mbed.h"
#include "MCP3204.h"
#include "24LC1025.h"

Serial pc(PA_2, PA_3, 115200); //pin19,20 TX,RX
CAN can(PA_11, PA_12); //pin21,22 rd,td
SPI spi(PA_7, PA_6, PA_5);
MCP3204 mcp1(spi,PB_0); //下z-
MCP3204 mcp2(spi,PB_1); //右x+
MCP3204 mcp3(spi,PB_3); //左x-
MCP3204 mcp4(spi,PA_8); // 前y+
MCP3204 mcp5(spi,PA_10); //上z+
MCP3204 mcp6(spi,PF_0); //後y-
LC1025 eeprom = LC1025(PB_7, PB_6);
DigitalOut led(PB_5);


CANMessage msg;

#define T 0.03

float cal_x=0,cal_y=0,cal_z=0;
int i,j,cnt=0,page_frg=0;
float world_time;
uint16_t eeprom_cnt = 0x0000;
char serialData[4];

union Float2Byte{
    float _float;
    char _byte[4];
};
typedef union Float2Byte Float2Byte;

union Int2Byte{
    unsigned short _int;
    char _byte[2];
};
typedef union Int2Byte Int2Byte;


void send(float senddata,int id){
    //pc.printf("Master send()\n\r");
    
    Float2Byte sendFloat;
    sendFloat._float = senddata;
    //ここに送りたい値を入れる．
    
    char serialData[4];
    for(int i=0;i<4;++i){
        serialData[i] = sendFloat._byte[i];
        //pc.printf("send_char: %d\n\r", serialData[i]);
    }
    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
    if(can.write(CANMessage(id, serialData, 4))){
        //pc.printf("Send.\n\r");
    } 
}

void send2(unsigned short senddata1,unsigned short senddata2,unsigned short senddata3,int id){
    //pc.printf("Master send()\n\r");
    
    /*ID: 0x01*/
    Int2Byte sendInt;
    sendInt._int = senddata1;
    //ここに送りたい値を入れる．
    
    char serialData[6];
    
    serialData[0] = sendInt._byte[0];
    serialData[1] = sendInt._byte[1];
        //pc.printf("send_char: %d\n\r", serialData[i]);
    sendInt._int = senddata2;
    serialData[2] = sendInt._byte[0];
    serialData[3] = sendInt._byte[1];

    sendInt._int = senddata3;
    serialData[4] = sendInt._byte[0];
    serialData[5] = sendInt._byte[1];
    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
    if(can.write(CANMessage(id, serialData, 6))){
        //pc.printf("Send.\n\r");
    } 
}

void receive(){
    Float2Byte getFloat;
    Int2Byte getInt;
    led = 0;
    
    if(can.read(msg)){
        //ID: 0x01
        if(msg.id == 0x01){
            //pc.printf("ID: 0x01\n\r");
            for(int i=0;i<4;++i){
                getFloat._byte[i] = msg.data[i];
                //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
            }
            world_time = getFloat._float;
            pc.printf("world_time:%.2f\r\n", world_time);
        }
        
        if(msg.id == 0x02){
            //pc.printf("ID: 0x02\n\r");
            getInt._byte[0] = msg.data[0];
            getInt._byte[1] = msg.data[1];
            cal_x=(float)getInt._int /10.0f;
            pc.printf("Mag:%.2f", cal_x);
            
            getInt._byte[0] = msg.data[2];
            getInt._byte[1] = msg.data[3];
            cal_y=(float)getInt._int /10.0f;
            pc.printf(",%.2f", cal_y);
            
            getInt._byte[0] = msg.data[4];
            getInt._byte[1] = msg.data[5];
            cal_z=(float)getInt._int /10.0f;
            pc.printf(",%.2f\r\n", cal_z);
            
            if(page_frg == 0){
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=0.0f;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                eeprom_cnt = 0x04 + eeprom_cnt;
                wait(0.01);
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=cal_x;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=cal_y;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=cal_z;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=world_time;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
                *(float*)serialData=0.0f;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                pc.printf("e:%d,0x%x\r\n",page_frg,eeprom_cnt);
            }else if(page_frg == 1){
                *(float*)serialData=0.0f;
                pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                *(float*)serialData=cal_x;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                *(float*)serialData=cal_y;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                *(float*)serialData=cal_z;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                *(float*)serialData=world_time;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                wait(0.01);
                eeprom_cnt = 0x04 + eeprom_cnt;
                *(float*)serialData=0.0f;
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                eeprom_cnt = 0x04 + eeprom_cnt;
                wait(0.01);            
            }else{
                pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
            }
            
            
        }
        
        if(msg.id == 0x03){
            //pc.printf("ID: 0x01\n\r");
            for(int i=0;i<4;++i){
                getFloat._byte[i] = msg.data[i];
                //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
            }
            pc.printf("ID0x03:%.2f\r\n", getFloat._float);
        }
        
        if(msg.id == 0x04){
            //pc.printf("ID: 0x01\n\r");
            for(int i=0;i<4;++i){
                getFloat._byte[i] = msg.data[i];
                //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
            }
            pc.printf("ID0x04:%.2f\r\n", getFloat._float);
        }
    }
    led=1;
    
}

int main(){
    int adc1[4],adc2[4],adc3[4],adc4[4],adc5[4],adc6[4];
    float ave[6],buff;
    
    can.attach(receive, CAN::RxIrq);
    pc.printf("Hello world%d\r\n",sizeof(i));
    led = 1;
    wait(1.0);
    *(float*)serialData=0.0f;
    for(i=0;i<6;i++){
        eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
        eeprom_cnt = 0x04 + eeprom_cnt;
        wait(0.1);
    }            
    
    while(1){
        adc1[0] = mcp1.sgl( CH0 );
        adc1[1] = mcp1.sgl( CH1 );
        adc1[2] = mcp1.sgl( CH2 );
        adc1[3] = mcp1.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc1[j-1] > adc1[j]){
                    buff = adc1[j-1];
                    adc1[j-1] = adc1[j];
                    adc1[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[0] = (adc1[1] + adc1[2])/2.0 + 120.0;
        
        adc2[0] = mcp2.sgl( CH0 );
        adc2[1] = mcp2.sgl( CH1 );
        adc2[2] = mcp2.sgl( CH2 );
        adc2[3] = mcp2.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc2[j-1] > adc2[j]){
                    buff = adc2[j-1];
                    adc2[j-1] = adc2[j];
                    adc2[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[1] = (adc2[1] + adc2[2])/2.0;
        
        adc3[0] = mcp3.sgl( CH0 );
        adc3[1] = mcp3.sgl( CH1 );
        adc3[2] = mcp3.sgl( CH2 );
        adc3[3] = mcp3.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc3[j-1] > adc3[j]){
                    buff = adc3[j-1];
                    adc3[j-1] = adc3[j];
                    adc3[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[2] = (adc3[1] + adc3[2])/2.0 + 70.0;
        
        adc4[0] = mcp4.sgl( CH0 );
        adc4[1] = mcp4.sgl( CH1 );
        adc4[2] = mcp4.sgl( CH2 );
        adc4[3] = mcp4.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc4[j-1] > adc4[j]){
                    buff = adc4[j-1];
                    adc4[j-1] = adc4[j];
                    adc4[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[3] = (adc4[1] + adc4[2])/2.0 + 55.0;
        
        adc5[0] = mcp5.sgl( CH0 );
        adc5[1] = mcp5.sgl( CH1 );
        adc5[2] = mcp5.sgl( CH2 );
        adc5[3] = mcp5.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc5[j-1] > adc5[j]){
                    buff = adc5[j-1];
                    adc5[j-1] = adc5[j];
                    adc5[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[4] = (adc5[1] + adc5[2])/2.0;
        
        adc6[0] = mcp6.sgl( CH0 );
        adc6[1] = mcp6.sgl( CH1 );
        adc6[2] = mcp6.sgl( CH2 );
        adc6[3] = mcp6.sgl( CH3 );
        for(i=0;i<4-1;i++){
            for(j=1;j<4-i;j++){
                if(adc6[j-1] > adc6[j]){
                    buff = adc6[j-1];
                    adc6[j-1] = adc6[j];
                    adc6[j] = buff;
                }
            }
        }
        //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
        ave[5] = (adc6[1] + adc6[2])/2.0 - 100.0;
        
        /*
        pc.printf("1");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc1[i]);
        }
        pc.printf(",%.1f\r\n", ave1);
        
        pc.printf("2");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc2[i]);
        }
        pc.printf(",%.1f\r\n", ave2);
        
        pc.printf("3");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc3[i]);
        }
        pc.printf(",%.1f\r\n", ave3);
        
        pc.printf("4");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc4[i]);
        }
        pc.printf(",%.1f\r\n", ave4);
        
        pc.printf("5");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc5[i]);
        }
        pc.printf(",%.1f\r\n", ave5);
        
        pc.printf("6");
        for(i=0; i<4; i++){
            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
            pc.printf(",%d",adc6[i]);
        }
        pc.printf(",%.1f\r\n", ave6);
        */
        
        cnt = 0;
        
        if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
            //else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] < ave[0])pc.printf("x:90,y:0,z:0\r\n");
        }else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] > ave[0]){
            send2((unsigned short)(180.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
            //else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:270,y:0,z:0\r\n");
        }else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
            //else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] < ave[0])pc.printf("x:0,y:0,z:90\r\n");
        }else if(ave[1] > ave[2] && ave[3] < ave[5] && ave[4] < ave[0]){
            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(180.0f+cal_z),6);
            //else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] < ave[0])pc.printf("x:0,y:0,z:270\r\n");
        }else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
            //else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:0,y:90,z:0\r\n");
        }else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] > ave[0]){
            send2((unsigned short)(0.0f+cal_x),(unsigned short)(180.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
            //else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:0,y:270,z:0\r\n");
        }else{
             pc.printf("none\r\n");
        }
        //pc.printf("%d,%x,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f\r\n",page_frg,eeprom_cnt,ave[1],ave[2],ave[3],ave[5],ave[4],ave[0]);
        
        for(i=0; i<6; i++){
            *(float*)serialData = ave[i];
            //pc.printf("%f,",ave[i]);
            if(eeprom_cnt > 0xFFF0){
                page_frg++;
                eeprom_cnt = 0;
            }
            
            if(page_frg == 0){
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
                pc.printf("%d,0x%x,%.1f\r\n",page_frg,eeprom_cnt,ave[i]);
            }else if(page_frg == 1){
                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
                //pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
            }else{
                //pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
            }
            eeprom_cnt = 0x04 + eeprom_cnt;
            wait(0.01);
            
        }
        wait(0.1);
    }
    
}

/*
int main(){
    int adc[4];
    int i;
    
    pc.printf("Hello world\r\n");
    wait(1.0);
    
    while(1){
        adc[0] = mcp.sgl( CH0 );
        adc[1] = mcp.sgl( CH1 );
        adc[2] = mcp.sgl( CH2 );
        adc[3] = mcp.sgl( CH3 );
        for(i=0; i<4; i++){
            pc.printf("CH%d:%d\r\n",i,adc[i]);
        }
        wait(0.5);
    }

}
*/