駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.h@12:f726eb78b54c, 2019-08-30 (annotated)

Committer:
nakedt555
Date:
Fri Aug 30 07:55:38 2019 +0000
Revision:
12:f726eb78b54c
Parent:
8:80708bacb5b5 
aaa

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