myCan.cpp

00001 #include "myCan.h"
00002 
00003 
00004 //This function run every 10 msec at main
00005 void My_Can::loop(){
00006     static uint32_t counter;
00007     if(enable_send_odom_){
00008         if((counter++ % 2) == 0){
00009             send_odom(); 
00010             led_toggle_.call();   
00011         }
00012     }
00013 }
00014 
00015 void My_Can::receive_cb(){
00016     CANMessage canMsg;
00017     if(read(canMsg)){         
00018         //ACKフレームの確認応答
00019         if (GetType(canMsg.id) == ACK_TYPE) {
00020             canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN);
00021             write(canMsg);
00022         }
00023         
00024         //PINGフレームの確認応答
00025         if (GetType(canMsg.id) == PING_TYPE) {
00026             canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN);
00027             write(canMsg);
00028         }
00029         
00030         switch(canMsg.data[0]){
00031         case ODOM_RESET:
00032             enable_send_odom_ = false;
00033             reset_odometry();
00034             break;  
00035             
00036         case ODOM_SET_INITIAL_XY:
00037         case ODOM_SET_INITIAL_ANGLE:
00038         case ODOM_SET_CONFIG:
00039             check_initial_frame(canMsg.data);
00040             break;
00041         }
00042     }   
00043 }
00044 
00045 void My_Can::check_initial_frame(uint8_t *data){
00046     static uint32_t ts[3] = {0.0, 0.0, 0.0};
00047     static can_odom_xy_t xy;
00048     static can_odom_angle_t angle;
00049     static can_odom_config_t config;
00050     
00051     switch(data[0]){
00052     case ODOM_SET_INITIAL_XY:
00053         for(int i = 0; i < 6; i++){
00054             xy.array[i] = data[i];   
00055         }
00056         ts[0] = timer_.read_ms();
00057         
00058         break;
00059         
00060     case ODOM_SET_INITIAL_ANGLE:
00061         for(int i = 0; i < 6; i++){
00062             angle.array[i] = data[i];  
00063         }
00064         ts[1] = timer_.read_ms();
00065     
00066         break;
00067     
00068     case ODOM_SET_CONFIG:
00069         for(int i = 0; i < 3; i++){
00070             config.array[i] = data[i];  
00071         }
00072         ts[2] = timer_.read_ms();
00073         
00074         break;
00075     }
00076     
00077     if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){
00078         return;
00079     }
00080     
00081     reset_odometry();
00082     
00083     Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle));
00084     set_initial_pose(initialpose);
00085     set_court_color((config.data.court_color == 0) ? COURT_RED : COURT_BLUE);
00086     set_enable_oled((config.data.enable_oled == 0) ? false : true);
00087     
00088     enable_send_odom_ = true; //CAN送信許可
00089     amcl_initialize_.call();
00090 }
00091 
00092 void My_Can::send_odom(){
00093     can_odom_xy_t odom_xy;
00094     can_odom_angle_t odom_angle;
00095 
00096     odom_xy.data.frameName = ODOM_DATA_XY;
00097     odom_angle.data.frameName = ODOM_DATA_ANGLE; 
00098     
00099     Vec3f pos = get_world();
00100     
00101     odom_xy.data.x = EncodeFixedNumber(pos.x());
00102     odom_xy.data.y = EncodeFixedNumber(pos.y());
00103     EncodeFloat(-pos.angle(), odom_angle.data.angle);//注意：マイナスをつけてる
00104 
00105     write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_xy.array, 6));
00106     wait_ms(1);
00107     write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_angle.array, 6));
00108 }