aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.cpp
- Committer:
- nakedt555
- Date:
- 2018-12-20
- Revision:
- 8:80708bacb5b5
- Parent:
- 7:b240464868e8
- Child:
- 9:b20145b7e1ba
File content as of revision 8:80708bacb5b5:
#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)); }