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