aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.cpp@8:80708bacb5b5, 2018-12-20 (annotated)
- Committer:
- nakedt555
- Date:
- Thu Dec 20 20:54:35 2018 +0000
- Revision:
- 8:80708bacb5b5
- Parent:
- 7:b240464868e8
- Child:
- 9:b20145b7e1ba
amcl kakunin
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 3:a45557a0dcb8 | 1 | #include "myCan.h" |
nakedt555 | 3:a45557a0dcb8 | 2 | |
nakedt555 | 6:20a32baeff79 | 3 | |
nakedt555 | 6:20a32baeff79 | 4 | //This function run every 10 msec at main |
nakedt555 | 6:20a32baeff79 | 5 | void My_Can::loop(){ |
nakedt555 | 8:80708bacb5b5 | 6 | static uint32_t counter; |
nakedt555 | 8:80708bacb5b5 | 7 | if(enable_send_odom_){ |
nakedt555 | 8:80708bacb5b5 | 8 | if((counter++ % 2) == 0){ |
nakedt555 | 8:80708bacb5b5 | 9 | send_odom(); |
nakedt555 | 8:80708bacb5b5 | 10 | led_toggle_.call(); |
nakedt555 | 8:80708bacb5b5 | 11 | } |
nakedt555 | 8:80708bacb5b5 | 12 | } |
nakedt555 | 5:e678f1ac6cdc | 13 | } |
nakedt555 | 5:e678f1ac6cdc | 14 | |
nakedt555 | 6:20a32baeff79 | 15 | void My_Can::receive_cb(){ |
nakedt555 | 5:e678f1ac6cdc | 16 | CANMessage canMsg; |
nakedt555 | 5:e678f1ac6cdc | 17 | if(read(canMsg)){ |
nakedt555 | 5:e678f1ac6cdc | 18 | //ACKフレームの確認応答 |
nakedt555 | 5:e678f1ac6cdc | 19 | if (GetType(canMsg.id) == ACK_TYPE) { |
nakedt555 | 5:e678f1ac6cdc | 20 | canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN); |
nakedt555 | 5:e678f1ac6cdc | 21 | write(canMsg); |
nakedt555 | 5:e678f1ac6cdc | 22 | } |
nakedt555 | 5:e678f1ac6cdc | 23 | |
nakedt555 | 5:e678f1ac6cdc | 24 | //PINGフレームの確認応答 |
nakedt555 | 5:e678f1ac6cdc | 25 | if (GetType(canMsg.id) == PING_TYPE) { |
nakedt555 | 5:e678f1ac6cdc | 26 | canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN); |
nakedt555 | 5:e678f1ac6cdc | 27 | write(canMsg); |
nakedt555 | 5:e678f1ac6cdc | 28 | } |
nakedt555 | 5:e678f1ac6cdc | 29 | |
nakedt555 | 5:e678f1ac6cdc | 30 | switch(canMsg.data[0]){ |
nakedt555 | 5:e678f1ac6cdc | 31 | case ODOM_RESET: |
nakedt555 | 8:80708bacb5b5 | 32 | enable_send_odom_ = false; |
nakedt555 | 8:80708bacb5b5 | 33 | reset_odometry(); |
nakedt555 | 5:e678f1ac6cdc | 34 | break; |
nakedt555 | 5:e678f1ac6cdc | 35 | |
nakedt555 | 5:e678f1ac6cdc | 36 | case ODOM_SET_INITIAL_XY: |
nakedt555 | 5:e678f1ac6cdc | 37 | case ODOM_SET_INITIAL_ANGLE: |
nakedt555 | 8:80708bacb5b5 | 38 | case ODOM_SET_CONFIG: |
nakedt555 | 5:e678f1ac6cdc | 39 | check_initial_frame(canMsg.data); |
nakedt555 | 5:e678f1ac6cdc | 40 | break; |
nakedt555 | 5:e678f1ac6cdc | 41 | } |
nakedt555 | 5:e678f1ac6cdc | 42 | } |
nakedt555 | 5:e678f1ac6cdc | 43 | } |
nakedt555 | 5:e678f1ac6cdc | 44 | |
nakedt555 | 5:e678f1ac6cdc | 45 | void My_Can::check_initial_frame(uint8_t *data){ |
nakedt555 | 8:80708bacb5b5 | 46 | static uint32_t ts[3] = {0.0, 0.0, 0.0}; |
nakedt555 | 5:e678f1ac6cdc | 47 | static can_odom_xy_t xy; |
nakedt555 | 5:e678f1ac6cdc | 48 | static can_odom_angle_t angle; |
nakedt555 | 8:80708bacb5b5 | 49 | static can_odom_config_t config; |
nakedt555 | 5:e678f1ac6cdc | 50 | |
nakedt555 | 5:e678f1ac6cdc | 51 | switch(data[0]){ |
nakedt555 | 5:e678f1ac6cdc | 52 | case ODOM_SET_INITIAL_XY: |
nakedt555 | 8:80708bacb5b5 | 53 | for(int i = 0; i < 6; i++){ |
nakedt555 | 5:e678f1ac6cdc | 54 | xy.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 55 | } |
nakedt555 | 5:e678f1ac6cdc | 56 | ts[0] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 57 | |
nakedt555 | 5:e678f1ac6cdc | 58 | break; |
nakedt555 | 5:e678f1ac6cdc | 59 | |
nakedt555 | 5:e678f1ac6cdc | 60 | case ODOM_SET_INITIAL_ANGLE: |
nakedt555 | 8:80708bacb5b5 | 61 | for(int i = 0; i < 6; i++){ |
nakedt555 | 6:20a32baeff79 | 62 | angle.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 63 | } |
nakedt555 | 5:e678f1ac6cdc | 64 | ts[1] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 65 | |
nakedt555 | 8:80708bacb5b5 | 66 | break; |
nakedt555 | 8:80708bacb5b5 | 67 | |
nakedt555 | 8:80708bacb5b5 | 68 | case ODOM_SET_CONFIG: |
nakedt555 | 8:80708bacb5b5 | 69 | for(int i = 0; i < 3; i++){ |
nakedt555 | 8:80708bacb5b5 | 70 | config.array[i] = data[i]; |
nakedt555 | 8:80708bacb5b5 | 71 | } |
nakedt555 | 8:80708bacb5b5 | 72 | ts[2] = timer_.read_ms(); |
nakedt555 | 8:80708bacb5b5 | 73 | |
nakedt555 | 8:80708bacb5b5 | 74 | break; |
nakedt555 | 5:e678f1ac6cdc | 75 | } |
nakedt555 | 5:e678f1ac6cdc | 76 | |
nakedt555 | 8:80708bacb5b5 | 77 | if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){ |
nakedt555 | 5:e678f1ac6cdc | 78 | return; |
nakedt555 | 5:e678f1ac6cdc | 79 | } |
nakedt555 | 5:e678f1ac6cdc | 80 | |
nakedt555 | 8:80708bacb5b5 | 81 | reset_odometry(); |
nakedt555 | 8:80708bacb5b5 | 82 | |
nakedt555 | 7:b240464868e8 | 83 | Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle)); |
nakedt555 | 5:e678f1ac6cdc | 84 | set_initial_pose(initialpose); |
nakedt555 | 8:80708bacb5b5 | 85 | set_court_color((config.data.court_color == 0) ? COURT_RED : COURT_BLUE); |
nakedt555 | 8:80708bacb5b5 | 86 | set_enable_oled((config.data.enable_oled == 0) ? false : true); |
nakedt555 | 8:80708bacb5b5 | 87 | |
nakedt555 | 8:80708bacb5b5 | 88 | enable_send_odom_ = true; //CAN送信許可 |
nakedt555 | 8:80708bacb5b5 | 89 | amcl_initialize_.call(); |
nakedt555 | 8:80708bacb5b5 | 90 | } |
nakedt555 | 8:80708bacb5b5 | 91 | |
nakedt555 | 8:80708bacb5b5 | 92 | void My_Can::send_odom(){ |
nakedt555 | 8:80708bacb5b5 | 93 | can_odom_xy_t odom_xy; |
nakedt555 | 8:80708bacb5b5 | 94 | can_odom_angle_t odom_angle; |
nakedt555 | 8:80708bacb5b5 | 95 | |
nakedt555 | 8:80708bacb5b5 | 96 | odom_xy.data.frameName = ODOM_DATA_XY; |
nakedt555 | 8:80708bacb5b5 | 97 | odom_angle.data.frameName = ODOM_DATA_ANGLE; |
nakedt555 | 8:80708bacb5b5 | 98 | |
nakedt555 | 8:80708bacb5b5 | 99 | Vec3f pos = get_world(); |
nakedt555 | 8:80708bacb5b5 | 100 | |
nakedt555 | 8:80708bacb5b5 | 101 | odom_xy.data.x = EncodeFixedNumber(pos.x()); |
nakedt555 | 8:80708bacb5b5 | 102 | odom_xy.data.y = EncodeFixedNumber(pos.y()); |
nakedt555 | 8:80708bacb5b5 | 103 | EncodeFloat(pos.angle(), odom_angle.data.angle); |
nakedt555 | 8:80708bacb5b5 | 104 | |
nakedt555 | 8:80708bacb5b5 | 105 | write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_xy.array, 6)); |
nakedt555 | 8:80708bacb5b5 | 106 | wait_ms(1); |
nakedt555 | 8:80708bacb5b5 | 107 | write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_angle.array, 6)); |
nakedt555 | 3:a45557a0dcb8 | 108 | } |