aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
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 }
Generated on Mon Jul 18 2022 12:07:51 by 1.7.2