Diff: myCan.h

Revision:

3:a45557a0dcb8

Child:

4:cf1a4e503974

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