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