aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myCan.h@5:e678f1ac6cdc, 2018-12-18 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 18 21:00:30 2018 +0000
- Revision:
- 5:e678f1ac6cdc
- Parent:
- 4:cf1a4e503974
- Child:
- 6:20a32baeff79
before can it
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 5:e678f1ac6cdc | 1 | |
nakedt555 | 3:a45557a0dcb8 | 2 | #ifndef _MY_CAN_H_ |
nakedt555 | 3:a45557a0dcb8 | 3 | #define _MY_CAN_H_ |
nakedt555 | 3:a45557a0dcb8 | 4 | |
nakedt555 | 3:a45557a0dcb8 | 5 | #include <mbed.h> |
nakedt555 | 3:a45557a0dcb8 | 6 | #include "odom.h" |
nakedt555 | 3:a45557a0dcb8 | 7 | #include "myRos.h" |
nakedt555 | 5:e678f1ac6cdc | 8 | #include "type.h" |
nakedt555 | 3:a45557a0dcb8 | 9 | |
nakedt555 | 3:a45557a0dcb8 | 10 | /* CAN_GLOBAL DEFINE BEGIN */ |
nakedt555 | 3:a45557a0dcb8 | 11 | |
nakedt555 | 3:a45557a0dcb8 | 12 | typedef uint16_t FIXED_NUM_16; |
nakedt555 | 3:a45557a0dcb8 | 13 | |
nakedt555 | 3:a45557a0dcb8 | 14 | typedef union{ |
nakedt555 | 3:a45557a0dcb8 | 15 | char array[8]; |
nakedt555 | 3:a45557a0dcb8 | 16 | |
nakedt555 | 3:a45557a0dcb8 | 17 | struct{ |
nakedt555 | 3:a45557a0dcb8 | 18 | uint8_t frameName; |
nakedt555 | 5:e678f1ac6cdc | 19 | uint8_t id; |
nakedt555 | 3:a45557a0dcb8 | 20 | FIXED_NUM_16 x; |
nakedt555 | 3:a45557a0dcb8 | 21 | FIXED_NUM_16 y; |
nakedt555 | 5:e678f1ac6cdc | 22 | uint32_t unuse : 16; |
nakedt555 | 3:a45557a0dcb8 | 23 | }data; |
nakedt555 | 3:a45557a0dcb8 | 24 | }can_odom_xy_t; |
nakedt555 | 3:a45557a0dcb8 | 25 | |
nakedt555 | 5:e678f1ac6cdc | 26 | //pose initializeとフレームを共用にするためcourt_colorを入れた |
nakedt555 | 5:e678f1ac6cdc | 27 | //通常のデータ送信では使わない |
nakedt555 | 3:a45557a0dcb8 | 28 | typedef union{ |
nakedt555 | 3:a45557a0dcb8 | 29 | char array[8]; |
nakedt555 | 3:a45557a0dcb8 | 30 | |
nakedt555 | 3:a45557a0dcb8 | 31 | struct{ |
nakedt555 | 3:a45557a0dcb8 | 32 | uint8_t frameName; |
nakedt555 | 5:e678f1ac6cdc | 33 | uint8_t id; |
nakedt555 | 3:a45557a0dcb8 | 34 | float angle; |
nakedt555 | 5:e678f1ac6cdc | 35 | uint8_t court_color; |
nakedt555 | 5:e678f1ac6cdc | 36 | uint32_t unuse : 8; |
nakedt555 | 3:a45557a0dcb8 | 37 | }data; |
nakedt555 | 3:a45557a0dcb8 | 38 | }can_odom_angle_t; |
nakedt555 | 3:a45557a0dcb8 | 39 | |
nakedt555 | 3:a45557a0dcb8 | 40 | #define NORMAL_TYPE (0x00) |
nakedt555 | 3:a45557a0dcb8 | 41 | #define PING_TYPE (0x01) |
nakedt555 | 3:a45557a0dcb8 | 42 | #define ACK_TYPE (0x02) |
nakedt555 | 3:a45557a0dcb8 | 43 | |
nakedt555 | 3:a45557a0dcb8 | 44 | #define ID_MAIN (0x00) |
nakedt555 | 3:a45557a0dcb8 | 45 | #define ID_ODOM (0x01) |
nakedt555 | 3:a45557a0dcb8 | 46 | |
nakedt555 | 3:a45557a0dcb8 | 47 | #define FIXED_NUM_INT_MAX 0x0f |
nakedt555 | 3:a45557a0dcb8 | 48 | #define CAN_FILTER_SID_MASK (0x07ff) |
nakedt555 | 3:a45557a0dcb8 | 49 | #define CAN_FILTER_TYP_MASK (0x0007) |
nakedt555 | 3:a45557a0dcb8 | 50 | #define CAN_FILTER_SND_MASK (0x0078) |
nakedt555 | 3:a45557a0dcb8 | 51 | #define CAN_FILTER_DST_MASK (0x0780) |
nakedt555 | 3:a45557a0dcb8 | 52 | |
nakedt555 | 5:e678f1ac6cdc | 53 | #define ODOM_DATA_XY (0xa0) |
nakedt555 | 5:e678f1ac6cdc | 54 | #define ODOM_DATA_ANGLE (0xa1) |
nakedt555 | 5:e678f1ac6cdc | 55 | #define ODOM_SET_INITIAL_XY (0xa2) |
nakedt555 | 5:e678f1ac6cdc | 56 | #define ODOM_SET_INITIAL_ANGLE (0xa3) |
nakedt555 | 5:e678f1ac6cdc | 57 | #define ODOM_RESET (0xa4) |
nakedt555 | 3:a45557a0dcb8 | 58 | /* CAN_GLOBAL DEFINE END */ |
nakedt555 | 3:a45557a0dcb8 | 59 | |
nakedt555 | 3:a45557a0dcb8 | 60 | class My_Can : public Odom_Abstract, CAN |
nakedt555 | 3:a45557a0dcb8 | 61 | { |
nakedt555 | 3:a45557a0dcb8 | 62 | private: |
nakedt555 | 3:a45557a0dcb8 | 63 | FunctionPointer amcl_initialize_; |
nakedt555 | 3:a45557a0dcb8 | 64 | FunctionPointer led_toggle_; |
nakedt555 | 5:e678f1ac6cdc | 65 | |
nakedt555 | 5:e678f1ac6cdc | 66 | Timer timer_; |
nakedt555 | 3:a45557a0dcb8 | 67 | |
nakedt555 | 3:a45557a0dcb8 | 68 | public: |
nakedt555 | 4:cf1a4e503974 | 69 | My_Can(Odom *odom) : Odom_Abstract(odom), CAN(PB_8, PB_9, 1000000){ |
nakedt555 | 3:a45557a0dcb8 | 70 | filter(CreateSid(0, ID_MAIN, ID_ODOM), CAN_FILTER_SND_MASK | CAN_FILTER_DST_MASK, CANStandard); |
nakedt555 | 5:e678f1ac6cdc | 71 | timer_.start(); |
nakedt555 | 3:a45557a0dcb8 | 72 | } |
nakedt555 | 3:a45557a0dcb8 | 73 | |
nakedt555 | 3:a45557a0dcb8 | 74 | void initialize_amcl_attach(My_Ros *object, void (My_Ros::*member)(void)){ |
nakedt555 | 3:a45557a0dcb8 | 75 | amcl_initialize_.attach(object, member); |
nakedt555 | 3:a45557a0dcb8 | 76 | } |
nakedt555 | 3:a45557a0dcb8 | 77 | |
nakedt555 | 3:a45557a0dcb8 | 78 | void led_toggle_attach(void (*function)(void)){ |
nakedt555 | 3:a45557a0dcb8 | 79 | led_toggle_.attach(function); |
nakedt555 | 3:a45557a0dcb8 | 80 | } |
nakedt555 | 3:a45557a0dcb8 | 81 | |
nakedt555 | 3:a45557a0dcb8 | 82 | void test(){ |
nakedt555 | 5:e678f1ac6cdc | 83 | // led_toggle_.call(); |
nakedt555 | 3:a45557a0dcb8 | 84 | } |
nakedt555 | 3:a45557a0dcb8 | 85 | |
nakedt555 | 3:a45557a0dcb8 | 86 | private: |
nakedt555 | 5:e678f1ac6cdc | 87 | |
nakedt555 | 5:e678f1ac6cdc | 88 | void receive(); |
nakedt555 | 5:e678f1ac6cdc | 89 | void check_initial_frame(uint8_t *data); |
nakedt555 | 3:a45557a0dcb8 | 90 | |
nakedt555 | 3:a45557a0dcb8 | 91 | FIXED_NUM_16 EncodeFixedNumber(float fn){ |
nakedt555 | 3:a45557a0dcb8 | 92 | uint16_t sign_part = 0; |
nakedt555 | 3:a45557a0dcb8 | 93 | uint16_t int_part = 0; |
nakedt555 | 3:a45557a0dcb8 | 94 | uint16_t frac_part = 0; |
nakedt555 | 3:a45557a0dcb8 | 95 | |
nakedt555 | 3:a45557a0dcb8 | 96 | //符号チェック |
nakedt555 | 3:a45557a0dcb8 | 97 | if(fn < 0){ |
nakedt555 | 3:a45557a0dcb8 | 98 | sign_part = 1; |
nakedt555 | 3:a45557a0dcb8 | 99 | fn = -fn; |
nakedt555 | 3:a45557a0dcb8 | 100 | } |
nakedt555 | 3:a45557a0dcb8 | 101 | |
nakedt555 | 3:a45557a0dcb8 | 102 | //サイズチェック |
nakedt555 | 3:a45557a0dcb8 | 103 | if(fn > FIXED_NUM_INT_MAX){ |
nakedt555 | 3:a45557a0dcb8 | 104 | //ERROR |
nakedt555 | 3:a45557a0dcb8 | 105 | fn = FIXED_NUM_INT_MAX; |
nakedt555 | 3:a45557a0dcb8 | 106 | } |
nakedt555 | 3:a45557a0dcb8 | 107 | |
nakedt555 | 3:a45557a0dcb8 | 108 | //整数部取得 |
nakedt555 | 3:a45557a0dcb8 | 109 | int_part = (uint16_t)fn; |
nakedt555 | 3:a45557a0dcb8 | 110 | |
nakedt555 | 3:a45557a0dcb8 | 111 | //小数以下取得 |
nakedt555 | 3:a45557a0dcb8 | 112 | float frac_tmp = fn - (float)int_part; |
nakedt555 | 3:a45557a0dcb8 | 113 | |
nakedt555 | 3:a45557a0dcb8 | 114 | //小数以下計算 |
nakedt555 | 3:a45557a0dcb8 | 115 | for(int i = 0; i < 11; i++){ |
nakedt555 | 3:a45557a0dcb8 | 116 | frac_tmp *= 2; |
nakedt555 | 3:a45557a0dcb8 | 117 | if(frac_tmp >= 1){ |
nakedt555 | 3:a45557a0dcb8 | 118 | frac_part |= (0x0400 >> i); |
nakedt555 | 3:a45557a0dcb8 | 119 | frac_tmp = frac_tmp - (int)frac_tmp; |
nakedt555 | 3:a45557a0dcb8 | 120 | } |
nakedt555 | 3:a45557a0dcb8 | 121 | } |
nakedt555 | 3:a45557a0dcb8 | 122 | |
nakedt555 | 3:a45557a0dcb8 | 123 | FIXED_NUM_16 ret = (sign_part << 15) | (int_part << 11) | frac_part; |
nakedt555 | 3:a45557a0dcb8 | 124 | return ret; |
nakedt555 | 3:a45557a0dcb8 | 125 | } |
nakedt555 | 3:a45557a0dcb8 | 126 | |
nakedt555 | 3:a45557a0dcb8 | 127 | float DecodeFixedNumber(FIXED_NUM_16 fn){ |
nakedt555 | 3:a45557a0dcb8 | 128 | float output = 0; |
nakedt555 | 3:a45557a0dcb8 | 129 | |
nakedt555 | 3:a45557a0dcb8 | 130 | for(int i = 0; i < 11; i++){ |
nakedt555 | 3:a45557a0dcb8 | 131 | if((fn & 0x07ff) & (0x0400 >> i)){ |
nakedt555 | 3:a45557a0dcb8 | 132 | output += 1.0f / (float)(1 << (i + 1)); |
nakedt555 | 3:a45557a0dcb8 | 133 | } |
nakedt555 | 3:a45557a0dcb8 | 134 | } |
nakedt555 | 3:a45557a0dcb8 | 135 | |
nakedt555 | 3:a45557a0dcb8 | 136 | output += (float)((fn & 0x7800) >> 11); |
nakedt555 | 3:a45557a0dcb8 | 137 | |
nakedt555 | 3:a45557a0dcb8 | 138 | return (fn & 0x8000) ? -output : output; |
nakedt555 | 3:a45557a0dcb8 | 139 | } |
nakedt555 | 3:a45557a0dcb8 | 140 | |
nakedt555 | 3:a45557a0dcb8 | 141 | /** |
nakedt555 | 3:a45557a0dcb8 | 142 | * @brief float型データをCAN用のuint8_t型データに変換する関数 |
nakedt555 | 3:a45557a0dcb8 | 143 | * @param float_data |
nakedt555 | 3:a45557a0dcb8 | 144 | * @param send_buf |
nakedt555 | 3:a45557a0dcb8 | 145 | */ |
nakedt555 | 3:a45557a0dcb8 | 146 | void EncodeFloat(float float_data, uint8_t *send_buf) { |
nakedt555 | 3:a45557a0dcb8 | 147 | union { |
nakedt555 | 3:a45557a0dcb8 | 148 | uint8_t byte[4]; |
nakedt555 | 3:a45557a0dcb8 | 149 | float floating; |
nakedt555 | 3:a45557a0dcb8 | 150 | } enc; |
nakedt555 | 3:a45557a0dcb8 | 151 | |
nakedt555 | 3:a45557a0dcb8 | 152 | enc.floating = float_data; |
nakedt555 | 3:a45557a0dcb8 | 153 | |
nakedt555 | 3:a45557a0dcb8 | 154 | send_buf[0] = enc.byte[0]; |
nakedt555 | 3:a45557a0dcb8 | 155 | send_buf[1] = enc.byte[1]; |
nakedt555 | 3:a45557a0dcb8 | 156 | send_buf[2] = enc.byte[2]; |
nakedt555 | 3:a45557a0dcb8 | 157 | send_buf[3] = enc.byte[3]; |
nakedt555 | 3:a45557a0dcb8 | 158 | } |
nakedt555 | 3:a45557a0dcb8 | 159 | |
nakedt555 | 3:a45557a0dcb8 | 160 | /** |
nakedt555 | 3:a45557a0dcb8 | 161 | * @brief CAN用のuint8_t型データをfloat型に変換する関数 |
nakedt555 | 3:a45557a0dcb8 | 162 | * @param receive_buf |
nakedt555 | 3:a45557a0dcb8 | 163 | * @return デコードされたデータ |
nakedt555 | 3:a45557a0dcb8 | 164 | */ |
nakedt555 | 3:a45557a0dcb8 | 165 | float DecodeFloat(uint8_t *receive_buf) { |
nakedt555 | 3:a45557a0dcb8 | 166 | union { |
nakedt555 | 3:a45557a0dcb8 | 167 | uint8_t byte[4]; |
nakedt555 | 3:a45557a0dcb8 | 168 | float floating; |
nakedt555 | 3:a45557a0dcb8 | 169 | } enc; |
nakedt555 | 3:a45557a0dcb8 | 170 | |
nakedt555 | 3:a45557a0dcb8 | 171 | enc.byte[0] = receive_buf[0]; |
nakedt555 | 3:a45557a0dcb8 | 172 | enc.byte[1] = receive_buf[1]; |
nakedt555 | 3:a45557a0dcb8 | 173 | enc.byte[2] = receive_buf[2]; |
nakedt555 | 3:a45557a0dcb8 | 174 | enc.byte[3] = receive_buf[3]; |
nakedt555 | 3:a45557a0dcb8 | 175 | |
nakedt555 | 3:a45557a0dcb8 | 176 | return enc.floating; |
nakedt555 | 3:a45557a0dcb8 | 177 | } |
nakedt555 | 3:a45557a0dcb8 | 178 | |
nakedt555 | 3:a45557a0dcb8 | 179 | /** |
nakedt555 | 3:a45557a0dcb8 | 180 | * @brief CANのSIDを作成 |
nakedt555 | 3:a45557a0dcb8 | 181 | * (MSB:宛先(4bit)、送り主(4bit)、データ型(3bit):LSB)で格納 |
nakedt555 | 3:a45557a0dcb8 | 182 | * @param type |
nakedt555 | 3:a45557a0dcb8 | 183 | * @param sender |
nakedt555 | 3:a45557a0dcb8 | 184 | * @param destination |
nakedt555 | 3:a45557a0dcb8 | 185 | * @return 作成したSID |
nakedt555 | 3:a45557a0dcb8 | 186 | */ |
nakedt555 | 3:a45557a0dcb8 | 187 | uint16_t CreateSid(uint8_t type, uint8_t sender, uint8_t destination) { |
nakedt555 | 3:a45557a0dcb8 | 188 | uint16_t msg = 0; |
nakedt555 | 3:a45557a0dcb8 | 189 | |
nakedt555 | 3:a45557a0dcb8 | 190 | type &= 0x07; |
nakedt555 | 3:a45557a0dcb8 | 191 | sender &= 0x0f; |
nakedt555 | 3:a45557a0dcb8 | 192 | destination &= 0x0f; |
nakedt555 | 3:a45557a0dcb8 | 193 | |
nakedt555 | 3:a45557a0dcb8 | 194 | msg = ((uint16_t) destination << 7); |
nakedt555 | 3:a45557a0dcb8 | 195 | msg += ((uint16_t) sender << 3); |
nakedt555 | 3:a45557a0dcb8 | 196 | msg += type; |
nakedt555 | 3:a45557a0dcb8 | 197 | return msg; |
nakedt555 | 3:a45557a0dcb8 | 198 | } |
nakedt555 | 3:a45557a0dcb8 | 199 | |
nakedt555 | 3:a45557a0dcb8 | 200 | /** |
nakedt555 | 3:a45557a0dcb8 | 201 | * @brief 受信したフレームのSIDからTypeを取得 |
nakedt555 | 3:a45557a0dcb8 | 202 | * @param receive_msg 受信したSID |
nakedt555 | 3:a45557a0dcb8 | 203 | * @return 抽出したType |
nakedt555 | 3:a45557a0dcb8 | 204 | */ |
nakedt555 | 3:a45557a0dcb8 | 205 | uint8_t GetType(uint16_t receive_msg) { |
nakedt555 | 3:a45557a0dcb8 | 206 | return (receive_msg & (0b0000000000000111)); |
nakedt555 | 3:a45557a0dcb8 | 207 | } |
nakedt555 | 3:a45557a0dcb8 | 208 | |
nakedt555 | 3:a45557a0dcb8 | 209 | /** |
nakedt555 | 3:a45557a0dcb8 | 210 | * @brief 受信したフレームのSIDからSenderを取得 |
nakedt555 | 3:a45557a0dcb8 | 211 | * @param receive_msg 受信したSID |
nakedt555 | 3:a45557a0dcb8 | 212 | * @return 抽出したSender |
nakedt555 | 3:a45557a0dcb8 | 213 | */ |
nakedt555 | 3:a45557a0dcb8 | 214 | uint8_t GetSender(uint16_t receive_msg) { |
nakedt555 | 3:a45557a0dcb8 | 215 | return ((receive_msg & (0b0000000001111000)) >> 3); |
nakedt555 | 3:a45557a0dcb8 | 216 | } |
nakedt555 | 3:a45557a0dcb8 | 217 | |
nakedt555 | 3:a45557a0dcb8 | 218 | /** |
nakedt555 | 3:a45557a0dcb8 | 219 | * @brief 受信したフレームのSIDからDestinationを取得 |
nakedt555 | 3:a45557a0dcb8 | 220 | * @param receive_msg 受信したSID |
nakedt555 | 3:a45557a0dcb8 | 221 | * @return 抽出したDestination |
nakedt555 | 3:a45557a0dcb8 | 222 | */ |
nakedt555 | 3:a45557a0dcb8 | 223 | uint8_t GetDestination(uint16_t receive_msg) { |
nakedt555 | 3:a45557a0dcb8 | 224 | return ((receive_msg & (0b0000011110000000)) >> 7); |
nakedt555 | 3:a45557a0dcb8 | 225 | } |
nakedt555 | 3:a45557a0dcb8 | 226 | |
nakedt555 | 3:a45557a0dcb8 | 227 | //Overlap function |
nakedt555 | 3:a45557a0dcb8 | 228 | virtual void loop(); |
nakedt555 | 3:a45557a0dcb8 | 229 | |
nakedt555 | 3:a45557a0dcb8 | 230 | }; |
nakedt555 | 3:a45557a0dcb8 | 231 | |
nakedt555 | 3:a45557a0dcb8 | 232 | #endif |