駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.h@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?

UserRevisionLine numberNew 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 6:20a32baeff79 34 uint8_t angle[4];
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 6:20a32baeff79 82 void can_rx_it_cb(void (*function)(void)){
nakedt555 6:20a32baeff79 83 attach(function);
nakedt555 6:20a32baeff79 84 }
nakedt555 6:20a32baeff79 85
nakedt555 3:a45557a0dcb8 86 void test(){
nakedt555 5:e678f1ac6cdc 87 // led_toggle_.call();
nakedt555 3:a45557a0dcb8 88 }
nakedt555 3:a45557a0dcb8 89
nakedt555 6:20a32baeff79 90 void receive_cb();
nakedt555 6:20a32baeff79 91
nakedt555 3:a45557a0dcb8 92 private:
nakedt555 5:e678f1ac6cdc 93
nakedt555 5:e678f1ac6cdc 94 void check_initial_frame(uint8_t *data);
nakedt555 3:a45557a0dcb8 95
nakedt555 3:a45557a0dcb8 96 FIXED_NUM_16 EncodeFixedNumber(float fn){
nakedt555 3:a45557a0dcb8 97 uint16_t sign_part = 0;
nakedt555 3:a45557a0dcb8 98 uint16_t int_part = 0;
nakedt555 3:a45557a0dcb8 99 uint16_t frac_part = 0;
nakedt555 3:a45557a0dcb8 100
nakedt555 3:a45557a0dcb8 101 //符号チェック
nakedt555 3:a45557a0dcb8 102 if(fn < 0){
nakedt555 3:a45557a0dcb8 103 sign_part = 1;
nakedt555 3:a45557a0dcb8 104 fn = -fn;
nakedt555 3:a45557a0dcb8 105 }
nakedt555 3:a45557a0dcb8 106
nakedt555 3:a45557a0dcb8 107 //サイズチェック
nakedt555 3:a45557a0dcb8 108 if(fn > FIXED_NUM_INT_MAX){
nakedt555 3:a45557a0dcb8 109 //ERROR
nakedt555 3:a45557a0dcb8 110 fn = FIXED_NUM_INT_MAX;
nakedt555 3:a45557a0dcb8 111 }
nakedt555 3:a45557a0dcb8 112
nakedt555 3:a45557a0dcb8 113 //整数部取得
nakedt555 3:a45557a0dcb8 114 int_part = (uint16_t)fn;
nakedt555 3:a45557a0dcb8 115
nakedt555 3:a45557a0dcb8 116 //小数以下取得
nakedt555 3:a45557a0dcb8 117 float frac_tmp = fn - (float)int_part;
nakedt555 3:a45557a0dcb8 118
nakedt555 3:a45557a0dcb8 119 //小数以下計算
nakedt555 3:a45557a0dcb8 120 for(int i = 0; i < 11; i++){
nakedt555 3:a45557a0dcb8 121 frac_tmp *= 2;
nakedt555 3:a45557a0dcb8 122 if(frac_tmp >= 1){
nakedt555 3:a45557a0dcb8 123 frac_part |= (0x0400 >> i);
nakedt555 3:a45557a0dcb8 124 frac_tmp = frac_tmp - (int)frac_tmp;
nakedt555 3:a45557a0dcb8 125 }
nakedt555 3:a45557a0dcb8 126 }
nakedt555 3:a45557a0dcb8 127
nakedt555 3:a45557a0dcb8 128 FIXED_NUM_16 ret = (sign_part << 15) | (int_part << 11) | frac_part;
nakedt555 3:a45557a0dcb8 129 return ret;
nakedt555 3:a45557a0dcb8 130 }
nakedt555 3:a45557a0dcb8 131
nakedt555 3:a45557a0dcb8 132 float DecodeFixedNumber(FIXED_NUM_16 fn){
nakedt555 3:a45557a0dcb8 133 float output = 0;
nakedt555 3:a45557a0dcb8 134
nakedt555 3:a45557a0dcb8 135 for(int i = 0; i < 11; i++){
nakedt555 3:a45557a0dcb8 136 if((fn & 0x07ff) & (0x0400 >> i)){
nakedt555 3:a45557a0dcb8 137 output += 1.0f / (float)(1 << (i + 1));
nakedt555 3:a45557a0dcb8 138 }
nakedt555 3:a45557a0dcb8 139 }
nakedt555 3:a45557a0dcb8 140
nakedt555 3:a45557a0dcb8 141 output += (float)((fn & 0x7800) >> 11);
nakedt555 3:a45557a0dcb8 142
nakedt555 3:a45557a0dcb8 143 return (fn & 0x8000) ? -output : output;
nakedt555 3:a45557a0dcb8 144 }
nakedt555 3:a45557a0dcb8 145
nakedt555 3:a45557a0dcb8 146 /**
nakedt555 3:a45557a0dcb8 147 * @brief float型データをCAN用のuint8_t型データに変換する関数
nakedt555 3:a45557a0dcb8 148 * @param float_data
nakedt555 3:a45557a0dcb8 149 * @param send_buf
nakedt555 3:a45557a0dcb8 150 */
nakedt555 3:a45557a0dcb8 151 void EncodeFloat(float float_data, uint8_t *send_buf) {
nakedt555 3:a45557a0dcb8 152 union {
nakedt555 3:a45557a0dcb8 153 uint8_t byte[4];
nakedt555 3:a45557a0dcb8 154 float floating;
nakedt555 3:a45557a0dcb8 155 } enc;
nakedt555 3:a45557a0dcb8 156
nakedt555 3:a45557a0dcb8 157 enc.floating = float_data;
nakedt555 3:a45557a0dcb8 158
nakedt555 3:a45557a0dcb8 159 send_buf[0] = enc.byte[0];
nakedt555 3:a45557a0dcb8 160 send_buf[1] = enc.byte[1];
nakedt555 3:a45557a0dcb8 161 send_buf[2] = enc.byte[2];
nakedt555 3:a45557a0dcb8 162 send_buf[3] = enc.byte[3];
nakedt555 3:a45557a0dcb8 163 }
nakedt555 3:a45557a0dcb8 164
nakedt555 3:a45557a0dcb8 165 /**
nakedt555 3:a45557a0dcb8 166 * @brief CAN用のuint8_t型データをfloat型に変換する関数
nakedt555 3:a45557a0dcb8 167 * @param receive_buf
nakedt555 3:a45557a0dcb8 168 * @return デコードされたデータ
nakedt555 3:a45557a0dcb8 169 */
nakedt555 3:a45557a0dcb8 170 float DecodeFloat(uint8_t *receive_buf) {
nakedt555 3:a45557a0dcb8 171 union {
nakedt555 3:a45557a0dcb8 172 uint8_t byte[4];
nakedt555 3:a45557a0dcb8 173 float floating;
nakedt555 3:a45557a0dcb8 174 } enc;
nakedt555 3:a45557a0dcb8 175
nakedt555 3:a45557a0dcb8 176 enc.byte[0] = receive_buf[0];
nakedt555 3:a45557a0dcb8 177 enc.byte[1] = receive_buf[1];
nakedt555 3:a45557a0dcb8 178 enc.byte[2] = receive_buf[2];
nakedt555 3:a45557a0dcb8 179 enc.byte[3] = receive_buf[3];
nakedt555 3:a45557a0dcb8 180
nakedt555 3:a45557a0dcb8 181 return enc.floating;
nakedt555 3:a45557a0dcb8 182 }
nakedt555 3:a45557a0dcb8 183
nakedt555 3:a45557a0dcb8 184 /**
nakedt555 3:a45557a0dcb8 185 * @brief CANのSIDを作成
nakedt555 3:a45557a0dcb8 186 * (MSB:宛先(4bit)、送り主(4bit)、データ型(3bit):LSB)で格納
nakedt555 3:a45557a0dcb8 187 * @param type
nakedt555 3:a45557a0dcb8 188 * @param sender
nakedt555 3:a45557a0dcb8 189 * @param destination
nakedt555 3:a45557a0dcb8 190 * @return 作成したSID
nakedt555 3:a45557a0dcb8 191 */
nakedt555 3:a45557a0dcb8 192 uint16_t CreateSid(uint8_t type, uint8_t sender, uint8_t destination) {
nakedt555 3:a45557a0dcb8 193 uint16_t msg = 0;
nakedt555 3:a45557a0dcb8 194
nakedt555 3:a45557a0dcb8 195 type &= 0x07;
nakedt555 3:a45557a0dcb8 196 sender &= 0x0f;
nakedt555 3:a45557a0dcb8 197 destination &= 0x0f;
nakedt555 3:a45557a0dcb8 198
nakedt555 3:a45557a0dcb8 199 msg = ((uint16_t) destination << 7);
nakedt555 3:a45557a0dcb8 200 msg += ((uint16_t) sender << 3);
nakedt555 3:a45557a0dcb8 201 msg += type;
nakedt555 3:a45557a0dcb8 202 return msg;
nakedt555 3:a45557a0dcb8 203 }
nakedt555 3:a45557a0dcb8 204
nakedt555 3:a45557a0dcb8 205 /**
nakedt555 3:a45557a0dcb8 206 * @brief 受信したフレームのSIDからTypeを取得
nakedt555 3:a45557a0dcb8 207 * @param receive_msg 受信したSID
nakedt555 3:a45557a0dcb8 208 * @return 抽出したType
nakedt555 3:a45557a0dcb8 209 */
nakedt555 3:a45557a0dcb8 210 uint8_t GetType(uint16_t receive_msg) {
nakedt555 3:a45557a0dcb8 211 return (receive_msg & (0b0000000000000111));
nakedt555 3:a45557a0dcb8 212 }
nakedt555 3:a45557a0dcb8 213
nakedt555 3:a45557a0dcb8 214 /**
nakedt555 3:a45557a0dcb8 215 * @brief 受信したフレームのSIDからSenderを取得
nakedt555 3:a45557a0dcb8 216 * @param receive_msg 受信したSID
nakedt555 3:a45557a0dcb8 217 * @return 抽出したSender
nakedt555 3:a45557a0dcb8 218 */
nakedt555 3:a45557a0dcb8 219 uint8_t GetSender(uint16_t receive_msg) {
nakedt555 3:a45557a0dcb8 220 return ((receive_msg & (0b0000000001111000)) >> 3);
nakedt555 3:a45557a0dcb8 221 }
nakedt555 3:a45557a0dcb8 222
nakedt555 3:a45557a0dcb8 223 /**
nakedt555 3:a45557a0dcb8 224 * @brief 受信したフレームのSIDからDestinationを取得
nakedt555 3:a45557a0dcb8 225 * @param receive_msg 受信したSID
nakedt555 3:a45557a0dcb8 226 * @return 抽出したDestination
nakedt555 3:a45557a0dcb8 227 */
nakedt555 3:a45557a0dcb8 228 uint8_t GetDestination(uint16_t receive_msg) {
nakedt555 3:a45557a0dcb8 229 return ((receive_msg & (0b0000011110000000)) >> 7);
nakedt555 3:a45557a0dcb8 230 }
nakedt555 3:a45557a0dcb8 231
nakedt555 3:a45557a0dcb8 232 //Overlap function
nakedt555 3:a45557a0dcb8 233 virtual void loop();
nakedt555 3:a45557a0dcb8 234
nakedt555 3:a45557a0dcb8 235 };
nakedt555 3:a45557a0dcb8 236
nakedt555 3:a45557a0dcb8 237 #endif