myCan.h

00001 
00002 #ifndef _MY_CAN_H_
00003 #define _MY_CAN_H_
00004 
00005 #include <mbed.h>
00006 #include "odom.h"
00007 #include "myRos.h"
00008 #include "type.h"
00009 
00010 /* CAN_GLOBAL DEFINE BEGIN */
00011 
00012 typedef uint16_t FIXED_NUM_16;
00013 
00014 typedef union{
00015     char array[6];
00016     
00017     struct{
00018         uint8_t frameName;
00019         FIXED_NUM_16 x;
00020         FIXED_NUM_16 y;
00021         uint8_t use_can_ack;
00022     }data;
00023 }can_odom_xy_t;
00024 
00025 typedef union{
00026     char array[6];
00027     
00028     struct{
00029         uint8_t frameName;
00030         uint8_t angle[4];
00031         uint8_t use_can_ack;
00032     }data;
00033 }can_odom_angle_t;
00034 
00035 typedef union{
00036     char array[3];
00037     
00038     struct{
00039         uint8_t frameName;
00040         uint8_t court_color : 1;
00041         uint8_t enable_oled : 1;
00042         uint8_t unuse : 6;
00043         uint8_t use_can_ack;
00044     }data;
00045 }can_odom_config_t;
00046 
00047 #define NORMAL_TYPE                 (0x00)
00048 #define PING_TYPE                   (0x01)
00049 #define ACK_TYPE                    (0x02)
00050 
00051 #define ID_MAIN                     (0x00)
00052 #define ID_ODOM                     (0x01)
00053 
00054 #define FIXED_NUM_INT_MAX           (0x0f)
00055 #define CAN_FILTER_SID_MASK         (0x07ff)
00056 #define CAN_FILTER_TYP_MASK         (0x0007)
00057 #define CAN_FILTER_SND_MASK         (0x0078)
00058 #define CAN_FILTER_DST_MASK         (0x0780)
00059 
00060 #define ODOM_DATA_XY                (0xa0)
00061 #define ODOM_DATA_ANGLE             (0xa1)
00062 #define ODOM_SET_INITIAL_XY         (0xa2)
00063 #define ODOM_SET_INITIAL_ANGLE      (0xa3)
00064 #define ODOM_SET_CONFIG             (0xa4)
00065 #define ODOM_RESET                  (0xa5)
00066 /* CAN_GLOBAL DEFINE END */
00067 
00068 class My_Can : public Odom_Abstract, CAN
00069 {
00070     private:
00071         FunctionPointer amcl_initialize_;
00072         FunctionPointer led_toggle_;
00073         
00074         Timer timer_;
00075         
00076         bool enable_send_odom_;
00077     
00078     public:
00079         My_Can(Odom *odom) : Odom_Abstract(odom), CAN(PB_8, PB_9, 1000000){
00080             filter(CreateSid(0, ID_MAIN, ID_ODOM), CAN_FILTER_SND_MASK | CAN_FILTER_DST_MASK, CANStandard);
00081             timer_.start();
00082             enable_send_odom_ = false;
00083         }
00084         
00085         void initialize_amcl_attach(My_Ros *object, void (My_Ros::*member)(void)){
00086             amcl_initialize_.attach(object, member);
00087         }
00088         
00089         void led_toggle_attach(void (*function)(void)){
00090             led_toggle_.attach(function);
00091         }
00092         
00093         void can_rx_it_cb(void (*function)(void)){
00094             attach(function);   
00095         }
00096         
00097         void test(){
00098 //            led_toggle_.call();
00099         }
00100         
00101         void receive_cb();
00102         
00103     private:
00104     
00105         void check_initial_frame(uint8_t *data);
00106         void send_odom();
00107         
00108         FIXED_NUM_16 EncodeFixedNumber(float fn){
00109             uint16_t sign_part = 0;
00110             uint16_t int_part = 0;
00111             uint16_t frac_part = 0;
00112             
00113             //符号チェック
00114             if(fn < 0){
00115                 sign_part = 1;
00116                 fn = -fn;
00117             }
00118             
00119             //サイズチェック
00120             if(fn > FIXED_NUM_INT_MAX){
00121                 //ERROR
00122                 fn = FIXED_NUM_INT_MAX;
00123             }
00124             
00125             //整数部取得
00126             int_part = (uint16_t)fn;
00127             
00128             //小数以下取得
00129             float frac_tmp = fn - (float)int_part;
00130             
00131             //小数以下計算
00132             for(int i = 0; i < 11; i++){
00133                 frac_tmp *= 2;
00134                 if(frac_tmp >= 1){
00135                     frac_part |= (0x0400 >> i);    
00136                     frac_tmp = frac_tmp - (int)frac_tmp;
00137                 }
00138             }
00139             
00140             FIXED_NUM_16 ret = (sign_part << 15) | (int_part << 11) | frac_part;
00141             return ret;
00142         }
00143         
00144         float DecodeFixedNumber(FIXED_NUM_16 fn){
00145             float output = 0;
00146         
00147             for(int i = 0; i < 11; i++){
00148                 if((fn & 0x07ff) & (0x0400 >> i)){
00149                     output += 1.0f / (float)(1 << (i + 1));
00150                 }
00151             }
00152             
00153             output += (float)((fn & 0x7800) >> 11);
00154             
00155             return (fn & 0x8000) ? -output : output;
00156         }
00157         
00158         /**
00159          * @brief float型データをCAN用のuint8_t型データに変換する関数
00160          * @param float_data
00161          * @param send_buf
00162          */
00163         void EncodeFloat(float float_data, uint8_t *send_buf) {
00164             union {
00165                 uint8_t byte[4];
00166                 float floating;
00167             } enc;
00168         
00169             enc.floating = float_data;
00170         
00171             send_buf[0] = enc.byte[0];
00172             send_buf[1] = enc.byte[1];
00173             send_buf[2] = enc.byte[2];
00174             send_buf[3] = enc.byte[3];
00175         }
00176         
00177         /**
00178          * @brief CAN用のuint8_t型データをfloat型に変換する関数
00179          * @param receive_buf
00180          * @return デコードされたデータ
00181          */
00182         float DecodeFloat(uint8_t *receive_buf) {
00183             union {
00184                 uint8_t byte[4];
00185                 float floating;
00186             } enc;
00187         
00188             enc.byte[0] = receive_buf[0];
00189             enc.byte[1] = receive_buf[1];
00190             enc.byte[2] = receive_buf[2];
00191             enc.byte[3] = receive_buf[3];
00192         
00193             return enc.floating;
00194         }
00195         
00196         /**
00197          * @brief CANのSIDを作成
00198          * (MSB:宛先(4bit)、送り主(4bit)、データ型(3bit):LSB)で格納
00199          * @param type
00200          * @param sender
00201          * @param destination
00202          * @return 作成したSID
00203          */
00204         uint16_t CreateSid(uint8_t type, uint8_t sender, uint8_t destination) {
00205             uint16_t msg = 0;
00206         
00207             type &= 0x07;
00208             sender &= 0x0f;
00209             destination &= 0x0f;
00210         
00211             msg = ((uint16_t) destination << 7);
00212             msg += ((uint16_t) sender << 3);
00213             msg += type;
00214             return msg;
00215         }
00216         
00217         /**
00218          * @brief 受信したフレームのSIDからTypeを取得
00219          * @param receive_msg 受信したSID
00220          * @return 抽出したType
00221          */
00222         uint8_t GetType(uint16_t receive_msg) {
00223             return (receive_msg & (0b0000000000000111));
00224         }
00225         
00226         /**
00227          * @brief 受信したフレームのSIDからSenderを取得
00228          * @param receive_msg 受信したSID
00229          * @return 抽出したSender
00230          */
00231         uint8_t GetSender(uint16_t receive_msg) {
00232             return ((receive_msg & (0b0000000001111000)) >> 3);
00233         }
00234         
00235         /**
00236          * @brief 受信したフレームのSIDからDestinationを取得
00237          * @param receive_msg 受信したSID
00238          * @return 抽出したDestination
00239          */
00240         uint8_t GetDestination(uint16_t receive_msg) {
00241             return ((receive_msg & (0b0000011110000000)) >> 7);
00242         }
00243         
00244         //Overlap function
00245         virtual void loop();
00246 
00247 };
00248 
00249 #endif