aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.cpp@6:20a32baeff79, 2018-12-18 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 18 21:51:13 2018 +0000
- Revision:
- 6:20a32baeff79
- Parent:
- 5:e678f1ac6cdc
- Child:
- 7:b240464868e8
kita
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 | 5:e678f1ac6cdc | 6 | |
nakedt555 | 5:e678f1ac6cdc | 7 | } |
nakedt555 | 5:e678f1ac6cdc | 8 | |
nakedt555 | 6:20a32baeff79 | 9 | void My_Can::receive_cb(){ |
nakedt555 | 5:e678f1ac6cdc | 10 | CANMessage canMsg; |
nakedt555 | 5:e678f1ac6cdc | 11 | if(read(canMsg)){ |
nakedt555 | 5:e678f1ac6cdc | 12 | //ACKフレームの確認応答 |
nakedt555 | 5:e678f1ac6cdc | 13 | if (GetType(canMsg.id) == ACK_TYPE) { |
nakedt555 | 5:e678f1ac6cdc | 14 | canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN); |
nakedt555 | 5:e678f1ac6cdc | 15 | write(canMsg); |
nakedt555 | 5:e678f1ac6cdc | 16 | } |
nakedt555 | 5:e678f1ac6cdc | 17 | |
nakedt555 | 5:e678f1ac6cdc | 18 | //PINGフレームの確認応答 |
nakedt555 | 5:e678f1ac6cdc | 19 | if (GetType(canMsg.id) == PING_TYPE) { |
nakedt555 | 5:e678f1ac6cdc | 20 | canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN); |
nakedt555 | 5:e678f1ac6cdc | 21 | write(canMsg); |
nakedt555 | 5:e678f1ac6cdc | 22 | } |
nakedt555 | 5:e678f1ac6cdc | 23 | |
nakedt555 | 5:e678f1ac6cdc | 24 | switch(canMsg.data[0]){ |
nakedt555 | 5:e678f1ac6cdc | 25 | case ODOM_RESET: |
nakedt555 | 5:e678f1ac6cdc | 26 | |
nakedt555 | 5:e678f1ac6cdc | 27 | break; |
nakedt555 | 5:e678f1ac6cdc | 28 | |
nakedt555 | 5:e678f1ac6cdc | 29 | case ODOM_SET_INITIAL_XY: |
nakedt555 | 5:e678f1ac6cdc | 30 | case ODOM_SET_INITIAL_ANGLE: |
nakedt555 | 5:e678f1ac6cdc | 31 | check_initial_frame(canMsg.data); |
nakedt555 | 5:e678f1ac6cdc | 32 | break; |
nakedt555 | 5:e678f1ac6cdc | 33 | } |
nakedt555 | 5:e678f1ac6cdc | 34 | } |
nakedt555 | 5:e678f1ac6cdc | 35 | } |
nakedt555 | 5:e678f1ac6cdc | 36 | |
nakedt555 | 5:e678f1ac6cdc | 37 | void My_Can::check_initial_frame(uint8_t *data){ |
nakedt555 | 5:e678f1ac6cdc | 38 | static uint32_t ts[2] = {0.0, 0.0}; |
nakedt555 | 5:e678f1ac6cdc | 39 | static can_odom_xy_t xy; |
nakedt555 | 5:e678f1ac6cdc | 40 | static can_odom_angle_t angle; |
nakedt555 | 6:20a32baeff79 | 41 | static float radian; |
nakedt555 | 5:e678f1ac6cdc | 42 | |
nakedt555 | 5:e678f1ac6cdc | 43 | switch(data[0]){ |
nakedt555 | 5:e678f1ac6cdc | 44 | case ODOM_SET_INITIAL_XY: |
nakedt555 | 5:e678f1ac6cdc | 45 | for(int i = 0; i < 8; i++){ |
nakedt555 | 5:e678f1ac6cdc | 46 | xy.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 47 | } |
nakedt555 | 5:e678f1ac6cdc | 48 | ts[0] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 49 | |
nakedt555 | 5:e678f1ac6cdc | 50 | break; |
nakedt555 | 5:e678f1ac6cdc | 51 | |
nakedt555 | 5:e678f1ac6cdc | 52 | case ODOM_SET_INITIAL_ANGLE: |
nakedt555 | 5:e678f1ac6cdc | 53 | for(int i = 0; i < 8; i++){ |
nakedt555 | 6:20a32baeff79 | 54 | angle.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 55 | } |
nakedt555 | 6:20a32baeff79 | 56 | radian = DecodeFloat(angle.data.angle); |
nakedt555 | 5:e678f1ac6cdc | 57 | ts[1] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 58 | |
nakedt555 | 5:e678f1ac6cdc | 59 | break; |
nakedt555 | 5:e678f1ac6cdc | 60 | } |
nakedt555 | 5:e678f1ac6cdc | 61 | |
nakedt555 | 6:20a32baeff79 | 62 | if(ABS(ts[0] - ts[1]) > 50){ |
nakedt555 | 5:e678f1ac6cdc | 63 | return; |
nakedt555 | 5:e678f1ac6cdc | 64 | } |
nakedt555 | 5:e678f1ac6cdc | 65 | |
nakedt555 | 5:e678f1ac6cdc | 66 | reset_pose(); |
nakedt555 | 6:20a32baeff79 | 67 | Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), radian); |
nakedt555 | 5:e678f1ac6cdc | 68 | set_initial_pose(initialpose); |
nakedt555 | 5:e678f1ac6cdc | 69 | set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE); |
nakedt555 | 5:e678f1ac6cdc | 70 | led_toggle_.call(); |
nakedt555 | 3:a45557a0dcb8 | 71 | } |