File content as of revision 9:b20145b7e1ba:

#include "myCan.h"


//This function run every 10 msec at main
void My_Can::loop(){
    static uint32_t counter;
    if(enable_send_odom_){
        if((counter++ % 2) == 0){
            send_odom(); 
            led_toggle_.call();   
        }
    }
}

void My_Can::receive_cb(){
    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:
            enable_send_odom_ = false;
            reset_odometry();
            break;  
            
        case ODOM_SET_INITIAL_XY:
        case ODOM_SET_INITIAL_ANGLE:
        case ODOM_SET_CONFIG:
            check_initial_frame(canMsg.data);
            break;
        }
    }   
}

void My_Can::check_initial_frame(uint8_t *data){
    static uint32_t ts[3] = {0.0, 0.0, 0.0};
    static can_odom_xy_t xy;
    static can_odom_angle_t angle;
    static can_odom_config_t config;
    
    switch(data[0]){
    case ODOM_SET_INITIAL_XY:
        for(int i = 0; i < 6; i++){
            xy.array[i] = data[i];   
        }
        ts[0] = timer_.read_ms();
        
        break;
        
    case ODOM_SET_INITIAL_ANGLE:
        for(int i = 0; i < 6; i++){
            angle.array[i] = data[i];  
        }
        ts[1] = timer_.read_ms();
    
        break;
    
    case ODOM_SET_CONFIG:
        for(int i = 0; i < 3; i++){
            config.array[i] = data[i];  
        }
        ts[2] = timer_.read_ms();
        
        break;
    }
    
    if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){
        return;
    }
    
    reset_odometry();
    
    Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle));
    set_initial_pose(initialpose);
    set_court_color((config.data.court_color == 0) ? COURT_RED : COURT_BLUE);
    set_enable_oled((config.data.enable_oled == 0) ? false : true);
    
    enable_send_odom_ = true; //CAN送信許可
    amcl_initialize_.call();
}

void My_Can::send_odom(){
    can_odom_xy_t odom_xy;
    can_odom_angle_t odom_angle;

    odom_xy.data.frameName = ODOM_DATA_XY;
    odom_angle.data.frameName = ODOM_DATA_ANGLE; 
    
    Vec3f pos = get_world();
    
    odom_xy.data.x = EncodeFixedNumber(pos.x());
    odom_xy.data.y = EncodeFixedNumber(pos.y());
    EncodeFloat(-pos.angle(), odom_angle.data.angle);//注意：マイナスをつけてる

    write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_xy.array, 6));
    wait_ms(1);
    write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_angle.array, 6));
}