motor_driver_bridge_mbed.h

00001 #pragma once
00002 #include "mbed.h"
00003 
00004 #include "motor_driver_bridge/motor_driver_struct.h"
00005 #include "motor_driver_bridge/can_define.h"
00006 #include "math_ops.h"
00007 
00008 /** IDについて : motor driverのCAN_IDは1,2,...だが、1始まりだとプログラムが面倒。
00009  *   なのでインターフェースとしてはmotor drive の id を 0,1,...とし、内部で1を足す実装とした
00010  **/
00011 class MotorDriverBridge
00012 {
00013 public:
00014     MotorDriverBridge() : can_(CAN_RD, CAN_TD), led(LED_PIN),led2(LED2), is_locked_(false){};
00015 
00016     void init(void (*callback_function)(const StateStruct &input), const int (&direction)[4])
00017     {
00018         callback_function_ = callback_function;
00019         // direction_ = direction;
00020         for (size_t i = 0; i < 4; i++)
00021         {
00022             direction_[i] = direction[i];
00023         }
00024 
00025         can_.frequency(CAN_BAUD_RATE);
00026         can_.attach(callback(this, &MotorDriverBridge::canCallback)); // attach 'CAN receive-complete' interrupt handler
00027         can_.filter(CAN_ID, 0xFFF, CANStandard, 0);                   // Set up can filter so it interrups only for messages with ID CAN_ID
00028     };
00029 
00030     void publish(const ControlStruct &control)
00031     {
00032         CANMessage txMsg;
00033         pack_cmd(control, txMsg);
00034         can_write(txMsg);
00035     };
00036     void enableMotor(int id, bool is_enable)
00037     {
00038         CANMessage txMsg;
00039         txMsg.id = listId2CANId(id);
00040         txMsg.data[0] = 0xFF;
00041         txMsg.data[1] = 0xFF;
00042         txMsg.data[2] = 0xFF;
00043         txMsg.data[3] = 0xFF;
00044         txMsg.data[4] = 0xFF;
00045         txMsg.data[5] = 0xFF;
00046         txMsg.data[6] = 0xFF;
00047         if (is_enable)
00048         {
00049             txMsg.data[7] = 0xFC;
00050             led = 1;
00051         }
00052         else
00053         {
00054             txMsg.data[7] = 0xFD;
00055             led = 0;
00056         }
00057         can_write(txMsg);
00058     };
00059 
00060     // void disableMotor(int id)
00061     // {
00062     //     CANMessage txMsg;
00063     //     txMsg.id = listId2CANId(id);
00064     //     txMsg.data[0] = 0xFF;
00065     //     txMsg.data[1] = 0xFF;
00066     //     txMsg.data[2] = 0xFF;
00067     //     txMsg.data[3] = 0xFF;
00068     //     txMsg.data[4] = 0xFF;
00069     //     txMsg.data[5] = 0xFF;
00070     //     txMsg.data[6] = 0xFF;
00071     //     txMsg.data[7] = 0xFD;
00072     //     can_write(txMsg);
00073     // }
00074 
00075     void setOrigin(int id)
00076     {
00077         CANMessage txMsg;
00078         txMsg.id = listId2CANId(id);
00079         txMsg.data[0] = 0xFF;
00080         txMsg.data[1] = 0xFF;
00081         txMsg.data[2] = 0xFF;
00082         txMsg.data[3] = 0xFF;
00083         txMsg.data[4] = 0xFF;
00084         txMsg.data[5] = 0xFF;
00085         txMsg.data[6] = 0xFF;
00086         txMsg.data[7] = 0xFE;
00087         can_write(txMsg);
00088     }
00089 
00090 private:
00091     CAN can_; // CAN Rx pin name, CAN Tx pin name
00092     DigitalOut led;
00093     DigitalOut led2;
00094     int direction_[4]; // motorの動く向き. 配列数が直打ちなのが微妙すぎる。
00095     int is_locked_;
00096     int i_int_old_[4];
00097 
00098     void (*callback_function_)(const StateStruct &input);
00099 
00100     ////IDについて : motor driverのCAN_IDは1,2,...だが、1始まりだとプログラムが面倒。なのでインターフェースとしてはmotor drive の id を 0,1,...とし、内部で1を足す実装とした
00101     int listId2CANId(int list_id)
00102     {
00103         return list_id + 1;
00104     }
00105     int CanId2ListId(int can_id)
00106     {
00107         return can_id - 1;
00108     }
00109     void canCallback()
00110     {
00111         CANMessage rxMsg;
00112         rxMsg.len = 6;
00113         can_.read(rxMsg); // read message into Rx message storage
00114 
00115         StateStruct data;
00116         unpack_reply(rxMsg, data);
00117         (*callback_function_)(data);
00118     };
00119 
00120     void unpack_reply(const CANMessage &rxMsg, StateStruct &state)
00121     {
00122         /// unpack ints from can buffer ///
00123         int id = rxMsg.data[0];
00124 
00125         int p_int = (rxMsg.data[1] << 8) | rxMsg.data[2];
00126         int v_int = (rxMsg.data[3] << 4) | (rxMsg.data[4] >> 4);
00127         int i_int = ((rxMsg.data[4] & 0xF) << 8) | rxMsg.data[5];
00128         /// convert unsigned ints to floats ///
00129         int list_id = CanId2ListId(id);
00130         int direction = direction_[list_id];
00131         float p = uint_to_float(p_int, P_MIN, P_MAX, 16) * direction;
00132         float v = uint_to_float(v_int, V_MIN, V_MAX, 12) * direction;
00133         float i = uint_to_float(i_int, -I_MAX, I_MAX, 12) * direction;
00134 
00135         state.id = CanId2ListId(id);
00136         state.position = p;
00137         state.velocity = v;
00138         state.torque = i;
00139 
00140         if(i_int != i_int_old_[list_id]){
00141             led = 1;
00142         }else{
00143             led = 0;
00144         }
00145         i_int_old_[list_id] = i_int;
00146     }
00147 
00148     void pack_cmd(const ControlStruct &control, CANMessage &txMsg)
00149     {
00150         /// limit data to be within bounds ///
00151         int direction = direction_[control.id];
00152         float p_des = fminf(fmaxf(P_MIN, control.p_des), P_MAX) * direction;
00153         float v_des = fminf(fmaxf(V_MIN, control.v_des), V_MAX) * direction;
00154         float kp = fminf(fmaxf(KP_MIN, control.kp), KP_MAX);
00155         float kd = fminf(fmaxf(KD_MIN, control.kd), KD_MAX);
00156         float torque_feed_forward = fminf(fmaxf(I_MIN, control.torque_feed_forward), I_MAX) * direction;
00157         /// convert floats to unsigned ints ///
00158         int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
00159         int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
00160         int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
00161         int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
00162         int t_int = float_to_uint(torque_feed_forward, I_MIN, I_MAX, 12);
00163         /// pack ints into the can buffer ///
00164         txMsg.data[0] = p_int >> 8;
00165         txMsg.data[1] = p_int & 0xFF;
00166         txMsg.data[2] = v_int >> 4;
00167         txMsg.data[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);
00168         txMsg.data[4] = kp_int & 0xFF;
00169         txMsg.data[5] = kd_int >> 4;
00170         txMsg.data[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);
00171         txMsg.data[7] = t_int & 0xff;
00172 
00173         txMsg.id = listId2CANId(control.id);
00174 
00175         if(txMsg.id == 4)
00176             led2 =1;
00177             
00178     }
00179     void can_write(const CANMessage &msg)
00180     {
00181         while (is_locked_)
00182         {
00183             wait(0.01);
00184         }
00185         is_locked_ = true;
00186         can_.write(msg);
00187         is_locked_ = false;
00188     }
00189 };