aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.cpp
- Committer:
- nakedt555
- Date:
- 2018-12-18
- Revision:
- 6:20a32baeff79
- Parent:
- 5:e678f1ac6cdc
- Child:
- 7:b240464868e8
File content as of revision 6:20a32baeff79:
#include "myCan.h" //This function run every 10 msec at main void My_Can::loop(){ } 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: break; case ODOM_SET_INITIAL_XY: case ODOM_SET_INITIAL_ANGLE: check_initial_frame(canMsg.data); break; } } } void My_Can::check_initial_frame(uint8_t *data){ static uint32_t ts[2] = {0.0, 0.0}; static can_odom_xy_t xy; static can_odom_angle_t angle; static float radian; switch(data[0]){ case ODOM_SET_INITIAL_XY: for(int i = 0; i < 8; i++){ xy.array[i] = data[i]; } ts[0] = timer_.read_ms(); break; case ODOM_SET_INITIAL_ANGLE: for(int i = 0; i < 8; i++){ angle.array[i] = data[i]; } radian = DecodeFloat(angle.data.angle); ts[1] = timer_.read_ms(); break; } if(ABS(ts[0] - ts[1]) > 50){ return; } reset_pose(); Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), radian); set_initial_pose(initialpose); set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE); led_toggle_.call(); }