駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.h@4:cf1a4e503974, 2018-12-12 (annotated)

Committer:
nakedt555
Date:
Wed Dec 12 03:35:52 2018 +0000
Revision:
4:cf1a4e503974
Parent:
3:a45557a0dcb8
Child:
5:e678f1ac6cdc
toriaezu ok;

Who changed what in which revision?

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