![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: myCan.h
- Revision:
- 3:a45557a0dcb8
- Child:
- 4:cf1a4e503974
diff -r 086272a2da1c -r a45557a0dcb8 myCan.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myCan.h Tue Dec 11 17:51:47 2018 +0000 @@ -0,0 +1,227 @@ +#ifndef _MY_CAN_H_ +#define _MY_CAN_H_ + +#include <mbed.h> +#include "odom.h" +#include "myRos.h" + +/* CAN_GLOBAL DEFINE BEGIN */ + +typedef uint16_t FIXED_NUM_16; + +typedef union{ + char array[8]; + + struct{ + uint8_t frameName; + uint32_t id : 24; + FIXED_NUM_16 x; + FIXED_NUM_16 y; + }data; +}can_odom_xy_t; + +typedef union{ + char array[8]; + + struct{ + uint8_t frameName; + uint32_t id : 24; + float angle; + }data; +}can_odom_angle_t; + +#define NORMAL_TYPE (0x00) +#define PING_TYPE (0x01) +#define ACK_TYPE (0x02) + +#define ID_MAIN (0x00) +#define ID_ODOM (0x01) + +#define FIXED_NUM_INT_MAX 0x0f +#define CAN_FILTER_SID_MASK (0x07ff) +#define CAN_FILTER_TYP_MASK (0x0007) +#define CAN_FILTER_SND_MASK (0x0078) +#define CAN_FILTER_DST_MASK (0x0780) + +#define ODOM_RESET (0xa0) +#define ODOM_BOARD_RESET (0xa1) +#define ODOM_BNO_RESET (0xa2) +#define ODOM_ROS_ENABLE (0xa3) +#define ODOM_SEQUENTIAL_START (0xa4) +#define ODOM_SEQUENTIAL_STOP (0xa5) +#define ODOM_DATA_XY (0xa6) +#define ODOM_DATA_ANGLE (0xa7) +#define ODOM_SET_ANGLE (0xa8) +/* CAN_GLOBAL DEFINE END */ + +class My_Can : public Odom_Abstract, CAN +{ + private: + FunctionPointer amcl_initialize_; + FunctionPointer led_toggle_; + + public: + My_Can(Odom *odom) : CAN(PB_8, PB_9, 1000000){ + set_instance(odom); + filter(CreateSid(0, ID_MAIN, ID_ODOM), CAN_FILTER_SND_MASK | CAN_FILTER_DST_MASK, CANStandard); + + Vec3f initialpose(-0.5, 0.5, 0); + set_initial_pose(initialpose); + } + + void initialize_amcl_attach(My_Ros *object, void (My_Ros::*member)(void)){ + amcl_initialize_.attach(object, member); + } + + void led_toggle_attach(void (*function)(void)){ + led_toggle_.attach(function); + } + + void test(){ + led_toggle_.call(); + } + + private: + + FIXED_NUM_16 EncodeFixedNumber(float fn){ + uint16_t sign_part = 0; + uint16_t int_part = 0; + uint16_t frac_part = 0; + + //符号チェック + if(fn < 0){ + sign_part = 1; + fn = -fn; + } + + //サイズチェック + if(fn > FIXED_NUM_INT_MAX){ + //ERROR + fn = FIXED_NUM_INT_MAX; + } + + //整数部取得 + int_part = (uint16_t)fn; + + //小数以下取得 + float frac_tmp = fn - (float)int_part; + + //小数以下計算 + for(int i = 0; i < 11; i++){ + frac_tmp *= 2; + if(frac_tmp >= 1){ + frac_part |= (0x0400 >> i); + frac_tmp = frac_tmp - (int)frac_tmp; + } + } + + FIXED_NUM_16 ret = (sign_part << 15) | (int_part << 11) | frac_part; + return ret; + } + + float DecodeFixedNumber(FIXED_NUM_16 fn){ + float output = 0; + + for(int i = 0; i < 11; i++){ + if((fn & 0x07ff) & (0x0400 >> i)){ + output += 1.0f / (float)(1 << (i + 1)); + } + } + + output += (float)((fn & 0x7800) >> 11); + + return (fn & 0x8000) ? -output : output; + } + + /** + * @brief float型データをCAN用のuint8_t型データに変換する関数 + * @param float_data + * @param send_buf + */ + void EncodeFloat(float float_data, uint8_t *send_buf) { + union { + uint8_t byte[4]; + float floating; + } enc; + + enc.floating = float_data; + + send_buf[0] = enc.byte[0]; + send_buf[1] = enc.byte[1]; + send_buf[2] = enc.byte[2]; + send_buf[3] = enc.byte[3]; + } + + /** + * @brief CAN用のuint8_t型データをfloat型に変換する関数 + * @param receive_buf + * @return デコードされたデータ + */ + float DecodeFloat(uint8_t *receive_buf) { + union { + uint8_t byte[4]; + float floating; + } enc; + + enc.byte[0] = receive_buf[0]; + enc.byte[1] = receive_buf[1]; + enc.byte[2] = receive_buf[2]; + enc.byte[3] = receive_buf[3]; + + return enc.floating; + } + + /** + * @brief CANのSIDを作成 + * (MSB:宛先(4bit)、送り主(4bit)、データ型(3bit):LSB)で格納 + * @param type + * @param sender + * @param destination + * @return 作成したSID + */ + uint16_t CreateSid(uint8_t type, uint8_t sender, uint8_t destination) { + uint16_t msg = 0; + + type &= 0x07; + sender &= 0x0f; + destination &= 0x0f; + + msg = ((uint16_t) destination << 7); + msg += ((uint16_t) sender << 3); + msg += type; + return msg; + } + + /** + * @brief 受信したフレームのSIDからTypeを取得 + * @param receive_msg 受信したSID + * @return 抽出したType + */ + uint8_t GetType(uint16_t receive_msg) { + return (receive_msg & (0b0000000000000111)); + } + + /** + * @brief 受信したフレームのSIDからSenderを取得 + * @param receive_msg 受信したSID + * @return 抽出したSender + */ + uint8_t GetSender(uint16_t receive_msg) { + return ((receive_msg & (0b0000000001111000)) >> 3); + } + + /** + * @brief 受信したフレームのSIDからDestinationを取得 + * @param receive_msg 受信したSID + * @return 抽出したDestination + */ + uint8_t GetDestination(uint16_t receive_msg) { + return ((receive_msg & (0b0000011110000000)) >> 7); + } + + //Overlap function + virtual void loop(); + +}; + +#endif \ No newline at end of file