aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.cpp@7:b240464868e8, 2018-12-18 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 18 22:06:34 2018 +0000
- Revision:
- 7:b240464868e8
- Parent:
- 6:20a32baeff79
- Child:
- 8:80708bacb5b5
12/19morning log;
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 | 5:e678f1ac6cdc | 41 | |
nakedt555 | 5:e678f1ac6cdc | 42 | switch(data[0]){ |
nakedt555 | 5:e678f1ac6cdc | 43 | case ODOM_SET_INITIAL_XY: |
nakedt555 | 5:e678f1ac6cdc | 44 | for(int i = 0; i < 8; i++){ |
nakedt555 | 5:e678f1ac6cdc | 45 | xy.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 46 | } |
nakedt555 | 5:e678f1ac6cdc | 47 | ts[0] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 48 | |
nakedt555 | 5:e678f1ac6cdc | 49 | break; |
nakedt555 | 5:e678f1ac6cdc | 50 | |
nakedt555 | 5:e678f1ac6cdc | 51 | case ODOM_SET_INITIAL_ANGLE: |
nakedt555 | 5:e678f1ac6cdc | 52 | for(int i = 0; i < 8; i++){ |
nakedt555 | 6:20a32baeff79 | 53 | angle.array[i] = data[i]; |
nakedt555 | 5:e678f1ac6cdc | 54 | } |
nakedt555 | 5:e678f1ac6cdc | 55 | ts[1] = timer_.read_ms(); |
nakedt555 | 5:e678f1ac6cdc | 56 | |
nakedt555 | 5:e678f1ac6cdc | 57 | break; |
nakedt555 | 5:e678f1ac6cdc | 58 | } |
nakedt555 | 5:e678f1ac6cdc | 59 | |
nakedt555 | 6:20a32baeff79 | 60 | if(ABS(ts[0] - ts[1]) > 50){ |
nakedt555 | 5:e678f1ac6cdc | 61 | return; |
nakedt555 | 5:e678f1ac6cdc | 62 | } |
nakedt555 | 5:e678f1ac6cdc | 63 | |
nakedt555 | 5:e678f1ac6cdc | 64 | reset_pose(); |
nakedt555 | 7:b240464868e8 | 65 | Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle)); |
nakedt555 | 5:e678f1ac6cdc | 66 | set_initial_pose(initialpose); |
nakedt555 | 5:e678f1ac6cdc | 67 | set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE); |
nakedt555 | 5:e678f1ac6cdc | 68 | led_toggle_.call(); |
nakedt555 | 3:a45557a0dcb8 | 69 | } |