aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: myCan.cpp
- Revision:
- 5:e678f1ac6cdc
- Parent:
- 3:a45557a0dcb8
- Child:
- 6:20a32baeff79
--- a/myCan.cpp Wed Dec 12 03:35:52 2018 +0000 +++ b/myCan.cpp Tue Dec 18 21:00:30 2018 +0000 @@ -1,6 +1,72 @@ #include "myCan.h" +void receive_func_cb(){ + +} + //This function run every 10 msec at main void My_Can::loop(){ + receive(); +} +void My_Can::receive(){ + 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; + + 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]; + } + ts[1] = timer_.read_ms(); + + break; + } + + if(ABS(ts[0] - ts[1]) > 100){ + return; + } + + reset_pose(); + Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), angle.data.angle); + set_initial_pose(initialpose); + set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE); + led_toggle_.call(); } \ No newline at end of file