go_origin_control.h

00001 #pragma once
00002 
00003 #include "motor_driver_bridge/motor_driver_struct.h"
00004 #include <catchrobo_msgs/MyRosCmd.h>
00005 
00006 class GoOriginControl
00007 {
00008 public:
00009     GoOriginControl() : is_arrived_(false){};
00010     void setOffset(const StateStruct &state, const catchrobo_msgs::MyRosCmd &cmd, float &offset)
00011     {
00012         offset = state.position - cmd.position;
00013     }
00014     // void setRosCmd(const catchrobo_msgs::MyRosCmd &cmd, const StateStruct &joint_state)
00015     // {
00016     //     target_ = cmd;
00017     //     is_arrived_ = false;
00018     // };
00019 
00020     // // dt間隔で呼ばれる想定
00021     // void getCmd(const StateStruct &state, const ControlStruct &except_command, ControlStruct &command, ControlResult::ControlResult &result, float &offset){
00022     //     //// 現在値にホールド
00023     //     //// [WARINIG] 面倒だったので、positionを原点出し時に到達する位置としています
00024     //     command.p_des = target_.position;
00025     //     command.v_des = 0;
00026     //     command.kp = target_.kp;
00027     //     command.kd = target_.kd;
00028     //     command.torque_feed_forward = target_.effort;
00029     //     if (is_arrived_)
00030     //     {
00031     //         result = ControlResult::RUNNING;
00032     //         return;
00033     //     }
00034     //     else
00035     //     {
00036     //         //// [WARINIG] 面倒だったので、acceleration_limitをしきい値とみなして実装しちゃってます
00037     //         if (fabs(state.torque) > target_.acceleration_limit)
00038     //         {
00039     //             result = ControlResult::FINISH;
00040     //             offset = state.position - target_.position;
00041     //             is_arrived_ = true;
00042     //             return;
00043     //         }
00044     //         else
00045     //         {
00046     //             //// 速度制御に変更
00047     //             command.kp = 0;
00048     //             command.v_des = target_.velocity;
00049     //         }
00050     //     }
00051     // };
00052 
00053 private:
00054     bool is_arrived_;
00055     catchrobo_msgs::MyRosCmd target_;
00056 };