駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

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

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