File content as of revision 5:e678f1ac6cdc:

#include "myCan.h"

void receive_func_cb(){

}

//This function run every 10 msec at main
void My_Can::loop(){
    receive();
}

void My_Can::receive(){
    CANMessage canMsg;
    if(read(canMsg)){         
        //ACKフレームの確認応答
        if (GetType(canMsg.id) == ACK_TYPE) {
            canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN);
            write(canMsg);
        }
        
        //PINGフレームの確認応答
        if (GetType(canMsg.id) == PING_TYPE) {
            canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN);
            write(canMsg);
        }
        
        switch(canMsg.data[0]){
        case ODOM_RESET:
            
            break;  
            
        case ODOM_SET_INITIAL_XY:
        case ODOM_SET_INITIAL_ANGLE:
            check_initial_frame(canMsg.data);
            break;
        }
    }   
}

void My_Can::check_initial_frame(uint8_t *data){
    static uint32_t ts[2] = {0.0, 0.0};
    static can_odom_xy_t xy;
    static can_odom_angle_t angle;
    
    switch(data[0]){
    case ODOM_SET_INITIAL_XY:
        for(int i = 0; i < 8; i++){
            xy.array[i] = data[i];   
        }
        ts[0] = timer_.read_ms();
        
        break;
        
    case ODOM_SET_INITIAL_ANGLE:
        for(int i = 0; i < 8; i++){
            angle.array[i] = data[i];   
        }
        ts[1] = timer_.read_ms();
    
        break;   
    }
    
    if(ABS(ts[0] - ts[1]) > 100){
        return;
    }
    
    reset_pose();
    Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), angle.data.angle);
    set_initial_pose(initialpose);
    set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE);
    led_toggle_.call(); 
}