駿 高世 / Mbed 2 deprecated abu2019_mr1

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?

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 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