aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: myCan.cpp
- Revision:
- 8:80708bacb5b5
- Parent:
- 7:b240464868e8
- Child:
- 9:b20145b7e1ba
--- a/myCan.cpp Tue Dec 18 22:06:34 2018 +0000 +++ b/myCan.cpp Thu Dec 20 20:54:35 2018 +0000 @@ -3,7 +3,13 @@ //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(){ @@ -23,11 +29,13 @@ 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; } @@ -35,13 +43,14 @@ } void My_Can::check_initial_frame(uint8_t *data){ - static uint32_t ts[2] = {0.0, 0.0}; + 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 < 8; i++){ + for(int i = 0; i < 6; i++){ xy.array[i] = data[i]; } ts[0] = timer_.read_ms(); @@ -49,21 +58,51 @@ break; case ODOM_SET_INITIAL_ANGLE: - for(int i = 0; i < 8; i++){ + for(int i = 0; i < 6; i++){ angle.array[i] = data[i]; } ts[1] = timer_.read_ms(); - break; + 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){ + if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){ return; } - reset_pose(); + reset_odometry(); + Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle)); set_initial_pose(initialpose); - set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE); - led_toggle_.call(); + 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)); } \ No newline at end of file